272 lines
13 KiB
Java
272 lines
13 KiB
Java
/* Copyright (c) 2023 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.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.hardware.camera.BuiltinCameraDirection;
|
|
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
|
import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl;
|
|
import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl;
|
|
import org.firstinspires.ftc.vision.VisionPortal;
|
|
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
|
|
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
|
|
|
|
import java.util.List;
|
|
import java.util.concurrent.TimeUnit;
|
|
|
|
/**
|
|
* This OpMode illustrates using a camera to locate and drive towards a specific AprilTag.
|
|
* The code assumes a basic two-wheel (Tank) Robot Drivetrain
|
|
*
|
|
* The driving goal is to rotate to keep the tag centered in the camera, while driving towards the tag to achieve the desired distance.
|
|
* To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS)
|
|
* You can determine the best exposure and gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder.
|
|
*
|
|
* The code assumes a Robot Configuration with motors named left_drive and right_drive.
|
|
* The motor directions must be set so a positive power goes forward on both wheels;
|
|
* This sample assumes that the default AprilTag Library (usually for the current season) is being loaded by default
|
|
* so you should choose to approach a valid tag ID (usually starting at 0)
|
|
*
|
|
* Under manual control, the left stick will move forward/back, and the right stick will rotate the robot.
|
|
* 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.
|
|
*
|
|
* Under "Drive To Target" mode, the robot has two goals:
|
|
* 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.)
|
|
* 2) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward)
|
|
*
|
|
* 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.
|
|
*
|
|
* Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder.
|
|
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
|
|
*
|
|
*/
|
|
|
|
@TeleOp(name="Tank Drive To AprilTag", group = "Concept")
|
|
@Disabled
|
|
public class RobotAutoDriveToAprilTagTank extends LinearOpMode
|
|
{
|
|
// Adjust these numbers to suit your robot.
|
|
final double DESIRED_DISTANCE = 12.0; // this is how close the camera should get to the target (inches)
|
|
|
|
// Set the GAIN constants to control the relationship between the measured position error, and how much power is
|
|
// applied to the drive motors to correct the error.
|
|
// Drive = Error * Gain Make these values smaller for smoother control, or larger for a more aggressive response.
|
|
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 MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot)
|
|
final double MAX_AUTO_TURN = 0.25; // Clip the turn speed to this max value (adjust for your robot)
|
|
|
|
private DcMotor leftDrive = null; // Used to control the left drive wheel
|
|
private DcMotor rightDrive = null; // Used to control the right drive wheel
|
|
|
|
private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera
|
|
private static final int DESIRED_TAG_ID = 0; // Choose the tag you want to approach or set to -1 for ANY tag.
|
|
private VisionPortal visionPortal; // Used to manage the video source.
|
|
private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process.
|
|
private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag
|
|
|
|
@Override public void runOpMode()
|
|
{
|
|
boolean targetFound = false; // Set to true when an AprilTag target is detected
|
|
double drive = 0; // Desired forward power/speed (-1 to +1) +ve is forward
|
|
double turn = 0; // Desired turning power/speed (-1 to +1) +ve is CounterClockwise
|
|
|
|
// Initialize the Apriltag Detection process
|
|
initAprilTag();
|
|
|
|
// Initialize the hardware variables. Note that the strings used here as parameters
|
|
// to 'get' must match 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.
|
|
// 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. Single Gear Reduction or 90 Deg drives may require direction flips
|
|
leftDrive.setDirection(DcMotor.Direction.REVERSE);
|
|
rightDrive.setDirection(DcMotor.Direction.FORWARD);
|
|
|
|
if (USE_WEBCAM)
|
|
setManualExposure(6, 250); // Use low exposure time to reduce motion blur
|
|
|
|
// Wait for the driver to press Start
|
|
telemetry.addData("Camera preview on/off", "3 dots, Camera Stream");
|
|
telemetry.addData(">", "Touch Play to start OpMode");
|
|
telemetry.update();
|
|
waitForStart();
|
|
|
|
while (opModeIsActive())
|
|
{
|
|
targetFound = false;
|
|
desiredTag = null;
|
|
|
|
// Step through the list of detected tags and look for a matching tag
|
|
List<AprilTagDetection> currentDetections = aprilTag.getDetections();
|
|
for (AprilTagDetection detection : currentDetections) {
|
|
if ((detection.metadata != null)
|
|
&& ((DESIRED_TAG_ID >= 0) || (detection.id == DESIRED_TAG_ID)) ){
|
|
targetFound = true;
|
|
desiredTag = detection;
|
|
break; // don't look any further.
|
|
}
|
|
}
|
|
|
|
// 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", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
|
|
telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range);
|
|
telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
|
|
} else {
|
|
telemetry.addData(">","Drive using joystick to find target\n");
|
|
}
|
|
|
|
// If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically .
|
|
if (gamepad1.left_bumper && targetFound) {
|
|
|
|
// Determine heading and range error so we can use them to control the robot automatically.
|
|
double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE);
|
|
double headingError = desiredTag.ftcPose.bearing;
|
|
|
|
// Use the speed and turn "gains" to calculate how we want the robot to move. Clip it to the maximum
|
|
drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED);
|
|
turn = Range.clip(headingError * TURN_GAIN, -MAX_AUTO_TURN, MAX_AUTO_TURN) ;
|
|
|
|
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();
|
|
|
|
// Apply desired axes motions to the drivetrain.
|
|
moveRobot(drive, turn);
|
|
sleep(10);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Move robot according to desired axes motions
|
|
* Positive X is forward
|
|
* Positive Yaw is counter-clockwise
|
|
*/
|
|
public void moveRobot(double x, double yaw) {
|
|
// Calculate left and right wheel powers.
|
|
double leftPower = x - yaw;
|
|
double rightPower = x + yaw;
|
|
|
|
// Normalize wheel powers to be less than 1.0
|
|
double max = Math.max(Math.abs(leftPower), Math.abs(rightPower));
|
|
if (max >1.0) {
|
|
leftPower /= max;
|
|
rightPower /= max;
|
|
}
|
|
|
|
// Send powers to the wheels.
|
|
leftDrive.setPower(leftPower);
|
|
rightDrive.setPower(rightPower);
|
|
}
|
|
|
|
/**
|
|
* Initialize the AprilTag processor.
|
|
*/
|
|
private void initAprilTag() {
|
|
// Create the AprilTag processor by using a builder.
|
|
aprilTag = new AprilTagProcessor.Builder().build();
|
|
|
|
// Create the vision portal by using a builder.
|
|
if (USE_WEBCAM) {
|
|
visionPortal = new VisionPortal.Builder()
|
|
.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
|
|
.addProcessor(aprilTag)
|
|
.build();
|
|
} else {
|
|
visionPortal = new VisionPortal.Builder()
|
|
.setCamera(BuiltinCameraDirection.BACK)
|
|
.addProcessor(aprilTag)
|
|
.build();
|
|
}
|
|
}
|
|
|
|
/*
|
|
Manually set the camera gain and exposure.
|
|
This can only be called AFTER calling initAprilTag(), and only works for Webcams;
|
|
*/
|
|
private void setManualExposure(int exposureMS, int gain) {
|
|
// Wait for the camera to be open, then use the controls
|
|
|
|
if (visionPortal == null) {
|
|
return;
|
|
}
|
|
|
|
// Make sure camera is streaming before we try to set the exposure controls
|
|
if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) {
|
|
telemetry.addData("Camera", "Waiting");
|
|
telemetry.update();
|
|
while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) {
|
|
sleep(20);
|
|
}
|
|
telemetry.addData("Camera", "Ready");
|
|
telemetry.update();
|
|
}
|
|
|
|
// Set camera controls unless we are stopping.
|
|
if (!isStopRequested())
|
|
{
|
|
ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class);
|
|
if (exposureControl.getMode() != ExposureControl.Mode.Manual) {
|
|
exposureControl.setMode(ExposureControl.Mode.Manual);
|
|
sleep(50);
|
|
}
|
|
exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS);
|
|
sleep(20);
|
|
GainControl gainControl = visionPortal.getCameraControl(GainControl.class);
|
|
gainControl.setGain(gain);
|
|
sleep(20);
|
|
telemetry.addData("Camera", "Ready");
|
|
telemetry.update();
|
|
}
|
|
}
|
|
}
|