diff --git a/teamcode/BlueBoardSide.java b/teamcode/BlueBoardSide.java new file mode 100644 index 0000000..6e4abe1 --- /dev/null +++ b/teamcode/BlueBoardSide.java @@ -0,0 +1,169 @@ +package org.firstinspires.ftc.teamcode; + +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.opMode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +@Autonomous(name="Blue, Board Side") +public class BlueBoardSide extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + public double tag; + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + // init(); + DcMotor frontLeftMotor = hardwareMap.dcMotor.get("fl"); + DcMotor backLeftMotor = hardwareMap.dcMotor.get("bl"); + DcMotor frontRightMotor = hardwareMap.dcMotor.get("fr"); + DcMotor backRightMotor = hardwareMap.dcMotor.get("br"); + + frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE); + backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE); + initAprilTag(); + + telemetry.update(); + + waitForStart(); + // start(); + if (isStopRequested()) return; + double speed = 0.5; + + while (opModeIsActive()) { + // loop(); + telemetry.update(); + frontRightMotor.setPower(speed); + frontLeftMotor.setPower(speed); + backRightMotor.setPower(speed); + backLeftMotor.setPower(speed); + + + telemetryAprilTag(); + if (gamepad1.a) { + frontRightMotor.setPower(speed); + frontLeftMotor.setPower(speed); + backRightMotor.setPower(speed); + backLeftMotor.setPower(speed); + } + else { + frontRightMotor.setPower(0); + frontLeftMotor.setPower(0); + backRightMotor.setPower(0); + backLeftMotor.setPower(0); + } + + if (gamepad1.x) { + frontRightMotor.setPower(0); + frontLeftMotor.setPower(speed); + backRightMotor.setPower(0); + backLeftMotor.setPower(speed); + } + else { + frontRightMotor.setPower(0); + frontLeftMotor.setPower(0); + backRightMotor.setPower(0); + backLeftMotor.setPower(0); + } + + if (gamepad1.b) { + frontRightMotor.setPower(0); + frontLeftMotor.setPower(speed); + backRightMotor.setPower(0); + backLeftMotor.setPower(speed); + } + else { + frontRightMotor.setPower(0); + frontLeftMotor.setPower(0); + backRightMotor.setPower(0); + backLeftMotor.setPower(0); + } + + telemetry.addData("--> ", tag); + if (tag == 1){ + telemetry.addData("--> ", "The First Tag!"); + //requestOpModeStop(); + } + else if (tag == 2){ + telemetry.addData("--> ", "The Second Tag!"); + requestOpModeStop(); + } + else if (tag == 2){ + telemetry.addData("--> ", "The Third Tag!"); + requestOpModeStop(); + } + + } + visionPortal.close(); + } + + private void initAprilTag() { + + // Create the AprilTag processor the easy way. + aprilTag = AprilTagProcessor.easyCreateWithDefaults(); + + // Create the vision portal the easy way. + if (USE_WEBCAM) { + visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, "Webcam 1"), aprilTag); + } else { + visionPortal = VisionPortal.easyCreateWithDefaults( + BuiltinCameraDirection.BACK, aprilTag); + } + + } // end method initAprilTag() + + /** + * Add telemetry about AprilTag detections. + */ + public void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + + tag = detection.id; + String name = detection.metadata.name; + telemetry.addData("id: ", tag); + } + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + } + diff --git a/teamcode/BlueFarSide.java b/teamcode/BlueFarSide.java new file mode 100644 index 0000000..ba3549e --- /dev/null +++ b/teamcode/BlueFarSide.java @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +@Autonomous(name="Blue, Far Side") +public class BlueFarSide extends LCore { + + @Override + public void runOpMode() { + + waitForStart(); + if (isStopRequested()) return; + + // move robot across april tags, stop robot when it finds the correct one + + + while (opModeIsActive()) { + + //telemetryAprilTag(); + } + visionPortal.close(); + } + +} \ No newline at end of file diff --git a/teamcode/FieldCentric.java b/teamcode/FieldCentric.java new file mode 100644 index 0000000..d42d91c --- /dev/null +++ b/teamcode/FieldCentric.java @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +@TeleOp(name="Field Centric") +public class FieldCentric extends OpMode { + @Override + public void init(){ + + } + + @Override + public void loop(){ + + } +} diff --git a/teamcode/LCore.java b/teamcode/LCore.java new file mode 100644 index 0000000..7cfab95 --- /dev/null +++ b/teamcode/LCore.java @@ -0,0 +1,86 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +//@Autonomous(name="Test Core L") +public class LCore extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + private AprilTagProcessor aprilTag; + public double tag; + public String name; + + public VisionPortal visionPortal; + @Override + public void runOpMode() { + } + + + + public void initAprilTag() { + + // Create the AprilTag processor the easy way. + aprilTag = AprilTagProcessor.easyCreateWithDefaults(); + + // Create the vision portal the easy way. + if (USE_WEBCAM) { + visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, "Webcam 1"), aprilTag); + } else { + visionPortal = VisionPortal.easyCreateWithDefaults( + BuiltinCameraDirection.BACK, aprilTag); + } + + } // end method initAprilTag() + + public void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + //telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } + else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + + tag = detection.id; + name = detection.metadata.name; + telemetry.addData("id: ", tag); + if (tag == 2){ + stop(); + } + } + + + + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() +} diff --git a/teamcode/RedBoardSide.java b/teamcode/RedBoardSide.java new file mode 100644 index 0000000..0434617 --- /dev/null +++ b/teamcode/RedBoardSide.java @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +@Autonomous(name="Red, Board Side") +public class RedBoardSide extends LCore { + + @Override + public void runOpMode() { + + waitForStart(); + if (isStopRequested()) return; + + // move robot across april tags, stop robot when it finds the correct one + + + while (opModeIsActive()) { + + //telemetryAprilTag(); + } + visionPortal.close(); + } + +} \ No newline at end of file diff --git a/teamcode/RedFarSide.java b/teamcode/RedFarSide.java new file mode 100644 index 0000000..ba84c59 --- /dev/null +++ b/teamcode/RedFarSide.java @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +@Autonomous(name="Red, Far Side") +public class RedFarSide extends LCore { + + @Override + public void runOpMode() { + + waitForStart(); + if (isStopRequested()) return; + + // move robot across april tags, stop robot when it finds the correct one + + + while (opModeIsActive()) { + + //telemetryAprilTag(); + } + visionPortal.close(); + } + +} \ No newline at end of file