From 01793bd37f7959d24c5347af71cd60305e46fdc5 Mon Sep 17 00:00:00 2001 From: heystuff Date: Mon, 29 Sep 2025 22:06:50 -0400 Subject: [PATCH 1/4] Create MecanumAprilTagCombo.java Added MecanumAprilTagCombo Code; currently unsure of its usage --- java/MecanumAprilTagCombo.java | 270 +++++++++++++++++++++++++++++++++ 1 file changed, 270 insertions(+) create mode 100644 java/MecanumAprilTagCombo.java diff --git a/java/MecanumAprilTagCombo.java b/java/MecanumAprilTagCombo.java new file mode 100644 index 0000000..c151f6b --- /dev/null +++ b/java/MecanumAprilTagCombo.java @@ -0,0 +1,270 @@ +// https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import java.util.concurrent.TimeUnit; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.CRServo; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +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; + +@TeleOp +public class MecanumAprilTagCombo extends LinearOpMode { + private DcMotor frontLeftMotor; + private DcMotor backLeftMotor; + private DcMotor frontRightMotor; + private DcMotor backRightMotor; + private DcMotor shootMotor; + private CRServo intakeServo; + + private double y; + private double x; + private double rx; + final double DESIRED_DISTANCE = 48.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". e.g. Ramp up to 50% power at a 25 inch error. (0.50 / 25.0) + final double TURN_GAIN = 0.01 ; // Turn Control "Gain". e.g. 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 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 = 24; // 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(); + + // Declare our motors + // Make sure your ID's match your configuration + DcMotor frontLeftMotor = hardwareMap.dcMotor.get("frontLeftMotor"); + DcMotor backLeftMotor = hardwareMap.dcMotor.get("backLeftMotor"); + DcMotor frontRightMotor = hardwareMap.dcMotor.get("frontRightMotor"); + DcMotor backRightMotor = hardwareMap.dcMotor.get("backRightMotor"); + frontLeftMotor = hardwareMap.dcMotor.get("frontLeftMotor"); + backLeftMotor = hardwareMap.dcMotor.get("backLeftMotor"); + frontRightMotor = hardwareMap.dcMotor.get("frontRightMotor"); + backRightMotor = hardwareMap.dcMotor.get("backRightMotor"); + shootMotor = hardwareMap.dcMotor.get("shootMotor"); + intakeServo = hardwareMap.crservo.get("intakeServo"); + + // Reverse the right side motors. This may be wrong for your setup. + // If your robot moves backwards when commanded to go forwards, + // reverse the left side instead. + // See the note about this earlier on this page. + // frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE); + // backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE); + frontLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE); + backLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE); + + // You may need to reverse the shooter motor or intake servo depending on how they are mounted. + // For example: + // shootMotor.setDirection(DcMotorSimple.Direction.REVERSE); + // intakeServo.setDirection(DcMotorSimple.Direction.REVERSE); + + + //if (USE_WEBCAM) + // setManualExposure(1, 300); // 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 START 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 currentDetections = aprilTag.getDetections(); + for (AprilTagDetection detection : currentDetections) { + // Look to see if we have size info on this tag. + if (detection.metadata != null) { + // Check to see if we want to track towards this tag. + if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) { + // Yes, we want to use this tag. + targetFound = true; + desiredTag = detection; + break; // don't look any further. + } else { + // This tag is in the library, but we do not want to track it right now. + telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id); + } + } else { + // This tag is NOT in the library, so we don't have enough information to track to it. + telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id); + } + } + + // Tell the driver what we see, and what to do. + if (targetFound) { + telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n"); + telemetry.addData("Found", "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("\n>","Drive using joysticks to find valid target\n"); + } + + // If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically . + + if (gamepad1.left_bumper) { + if (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 + y = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED); + //x + rx = Range.clip(headingError * TURN_GAIN, -MAX_AUTO_TURN, MAX_AUTO_TURN) ; + + telemetry.addData("Auto","Drive %5.2f, Turn %5.2f", drive, turn); + } else { + frontLeftMotor.setPower(0); + frontRightMotor.setPower(0); + backLeftMotor.setPower(0); + backRightMotor.setPower(0); + } + + } else { + + // drive using manual POV Joystick mode. + y = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%. + x = gamepad1.left_stick_x * 1.1; // to counteract imperfect strafing; + rx = -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(y, x, rx); + sleep(10); + } + } + + /** + * Move robot according to desired axes motions + *

+ * Positive X is forward + *

+ * Positive Yaw is counter-clockwise + */ + public void moveRobot(double y, double x, double rx) { + // Calculate left and right wheel powers. + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + + + // Send powers to the wheels. + frontLeftMotor.setPower(frontLeftPower); + backLeftMotor.setPower(backLeftPower); + frontRightMotor.setPower(frontRightPower); + backRightMotor.setPower(backRightPower); + +} + +/** + * Initialize the AprilTag processor. + */ +private void initAprilTag() { + // Create the AprilTag processor by using a builder. + aprilTag = new AprilTagProcessor.Builder().build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // e.g. Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second + // Note: Decimation can be changed on-the-fly to adapt during a match. + aprilTag.setDecimation(2); + + // 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(); + } +} +} From b482091904922ab17b54bb9265cd8ebe8eb0f080 Mon Sep 17 00:00:00 2001 From: heystuff Date: Mon, 29 Sep 2025 22:17:30 -0400 Subject: [PATCH 2/4] Update MecanumAprilTagCombo.java changed code so that it wouldn't bug out as much. gnight --- java/MecanumAprilTagCombo.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/java/MecanumAprilTagCombo.java b/java/MecanumAprilTagCombo.java index c151f6b..5a2da20 100644 --- a/java/MecanumAprilTagCombo.java +++ b/java/MecanumAprilTagCombo.java @@ -31,12 +31,12 @@ public class MecanumAprilTagCombo extends LinearOpMode { private DcMotor backLeftMotor; private DcMotor frontRightMotor; private DcMotor backRightMotor; - private DcMotor shootMotor; - private CRServo intakeServo; + //private DcMotor shootMotor; + //private CRServo intakeServo; - private double y; - private double x; - private double rx; + private double y = 0; + private double x = 0; + private double rx = 0; final double DESIRED_DISTANCE = 48.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 @@ -73,8 +73,8 @@ public class MecanumAprilTagCombo extends LinearOpMode { backLeftMotor = hardwareMap.dcMotor.get("backLeftMotor"); frontRightMotor = hardwareMap.dcMotor.get("frontRightMotor"); backRightMotor = hardwareMap.dcMotor.get("backRightMotor"); - shootMotor = hardwareMap.dcMotor.get("shootMotor"); - intakeServo = hardwareMap.crservo.get("intakeServo"); + //shootMotor = hardwareMap.dcMotor.get("shootMotor"); + //intakeServo = hardwareMap.crservo.get("intakeServo"); // Reverse the right side motors. This may be wrong for your setup. // If your robot moves backwards when commanded to go forwards, From f68678e4e61e0db0c2e1ca0f93cb1fdf9ac43b32 Mon Sep 17 00:00:00 2001 From: heystuff Date: Fri, 3 Oct 2025 22:00:58 -0400 Subject: [PATCH 3/4] Update MecanumAprilTagCombo.java Added Horizontal Strafing --- java/MecanumAprilTagCombo.java | 107 ++++++++++++++++++++++++--------- 1 file changed, 78 insertions(+), 29 deletions(-) diff --git a/java/MecanumAprilTagCombo.java b/java/MecanumAprilTagCombo.java index 5a2da20..d3670e6 100644 --- a/java/MecanumAprilTagCombo.java +++ b/java/MecanumAprilTagCombo.java @@ -2,6 +2,8 @@ package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import java.util.concurrent.TimeUnit; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -42,10 +44,10 @@ public class MecanumAprilTagCombo extends LinearOpMode { // 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". e.g. Ramp up to 50% power at a 25 inch error. (0.50 / 25.0) - final double TURN_GAIN = 0.01 ; // Turn Control "Gain". e.g. Ramp up to 25% power at a 25 degree error. (0.25 / 25.0) + final double SPEED_GAIN = 0.035 ; // Speed Control "Gain". e.g. Ramp up to 50% power at a 25 inch error. (0.50 / 25.0) + final double TURN_GAIN = 0.017; // Turn Control "Gain". e.g. 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_SPEED = 0.75; // 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 static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera @@ -91,8 +93,8 @@ public class MecanumAprilTagCombo extends LinearOpMode { // intakeServo.setDirection(DcMotorSimple.Direction.REVERSE); - //if (USE_WEBCAM) - // setManualExposure(1, 300); // Use low exposure time to reduce motion blur + 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"); @@ -132,6 +134,7 @@ public class MecanumAprilTagCombo extends LinearOpMode { telemetry.addData("Found", "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); + telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw); } else { telemetry.addData("\n>","Drive using joysticks to find valid target\n"); } @@ -142,33 +145,79 @@ public class MecanumAprilTagCombo extends LinearOpMode { if (targetFound) { // Determine heading and range error so we can use them to control the robot automatically. double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE); + double yawError = (desiredTag.ftcPose.yaw); 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 y = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED); - //x - rx = Range.clip(headingError * TURN_GAIN, -MAX_AUTO_TURN, MAX_AUTO_TURN) ; - - telemetry.addData("Auto","Drive %5.2f, Turn %5.2f", drive, turn); + x = Range.clip(yawError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED); + rx = -Range.clip(headingError * TURN_GAIN, -MAX_AUTO_TURN, MAX_AUTO_TURN) ; + + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + frontLeftMotor.setPower(frontLeftPower); + backLeftMotor.setPower(backLeftPower); + frontRightMotor.setPower(frontRightPower); + backRightMotor.setPower(backRightPower); + telemetry.addData("Auto","Drive %5.2f, Turn %5.2f", y, rx); } else { - frontLeftMotor.setPower(0); - frontRightMotor.setPower(0); - backLeftMotor.setPower(0); - backRightMotor.setPower(0); + y = 0; + x = 0; + rx = 0; + + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + frontLeftMotor.setPower(frontLeftPower); + backLeftMotor.setPower(backLeftPower); + frontRightMotor.setPower(frontRightPower); + backRightMotor.setPower(backRightPower); + telemetry.addData("Auto","Drive %5.2f, Turn %5.2f", y, rx); } } else { // drive using manual POV Joystick mode. - y = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%. + y = -gamepad1.left_stick_y; // Reduce drive rate to 50%. x = gamepad1.left_stick_x * 1.1; // to counteract imperfect strafing; - rx = -gamepad1.right_stick_x / 4.0; // Reduce turn rate to 25%. + rx = gamepad1.right_stick_x; // Reduce turn rate to 25%. telemetry.addData("Manual","Drive %5.2f, Turn %5.2f", drive, turn); + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if at least one is out of the range [-1, 1] + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + frontLeftMotor.setPower(frontLeftPower); + backLeftMotor.setPower(backLeftPower); + frontRightMotor.setPower(frontRightPower); + backRightMotor.setPower(backRightPower); } telemetry.update(); - // Apply desired axes motions to the drivetrain. - moveRobot(y, x, rx); + + + + // --- Intake and Shooter Control --- + // The triggers return a value from 0.0 to 1.0, which is perfect for continuous power + //double intakePower = gamepad1.left_trigger; + // double shootPower = gamepad1.right_trigger; + + // Set power to the intake servo and shooter motor + // intakeServo.setPower(intakePower); + // shootMotor.setPower(shootPower); + sleep(10); } } @@ -180,23 +229,23 @@ public class MecanumAprilTagCombo extends LinearOpMode { *

* Positive Yaw is counter-clockwise */ - public void moveRobot(double y, double x, double rx) { - // Calculate left and right wheel powers. - double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); - double frontLeftPower = (y + x + rx) / denominator; - double backLeftPower = (y - x + rx) / denominator; - double frontRightPower = (y - x - rx) / denominator; - double backRightPower = (y + x - rx) / denominator; +// public void moveRobot(double y, double x, double rx) { +// // Calculate left and right wheel powers. +// double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); +// double frontLeftPower = (y + x + rx) / denominator; +// double backLeftPower = (y - x + rx) / denominator; +// double frontRightPower = (y - x - rx) / denominator; +// double backRightPower = (y + x - rx) / denominator; // Send powers to the wheels. - frontLeftMotor.setPower(frontLeftPower); - backLeftMotor.setPower(backLeftPower); - frontRightMotor.setPower(frontRightPower); - backRightMotor.setPower(backRightPower); +// frontLeftMotor.setPower(frontLeftPower); +// backLeftMotor.setPower(backLeftPower); +// frontRightMotor.setPower(frontRightPower); +// backRightMotor.setPower(backRightPower); -} +//} /** * Initialize the AprilTag processor. From e75a6d03a0eb3e6d05eeea4b359cef5ea885fe5b Mon Sep 17 00:00:00 2001 From: heystuff Date: Sat, 4 Oct 2025 21:08:23 -0400 Subject: [PATCH 4/4] Update MecanumAprilTagCombo.java --- java/MecanumAprilTagCombo.java | 1 - 1 file changed, 1 deletion(-) diff --git a/java/MecanumAprilTagCombo.java b/java/MecanumAprilTagCombo.java index d3670e6..ef8873a 100644 --- a/java/MecanumAprilTagCombo.java +++ b/java/MecanumAprilTagCombo.java @@ -1,4 +1,3 @@ -// https://gm0.org/en/latest/docs/software/tutorials/mecanum-drive.html package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.Disabled;