diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RobotAutoDriveByEncoder_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RobotAutoDriveByEncoder_Linear.java
new file mode 100644
index 0000000000..1d0f9fd6b0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RobotAutoDriveByEncoder_Linear.java
@@ -0,0 +1,187 @@
+/* Copyright (c) 2017 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.teamcode.auto;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+/*
+ * This OpMode illustrates the concept of driving a path based on encoder counts.
+ * The code is structured as a LinearOpMode
+ *
+ * The code REQUIRES that you DO have encoders on the wheels,
+ * otherwise you would use: RobotAutoDriveByTime;
+ *
+ * This code ALSO requires that the drive Motors have been configured such that a positive
+ * power command moves them forward, and causes the encoders to count UP.
+ *
+ * The desired path in this example is:
+ * - Drive forward for 48 inches
+ * - Spin right for 12 Inches
+ * - Drive Backward for 24 inches
+ * - Stop and close the claw.
+ *
+ * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS)
+ * that performs the actual movement.
+ * This method assumes that each movement is relative to the last stopping place.
+ * There are other ways to perform encoder based moves, but this method is probably the simplest.
+ * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile
+ *
+ * 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
+ */
+
+@Autonomous(name="Robot: Auto Drive By Encoder", group="Robot")
+@Disabled
+public class RobotAutoDriveByEncoder_Linear extends LinearOpMode {
+
+ /* Declare OpMode members. */
+ private DcMotor leftDrive = null;
+ private DcMotor rightDrive = null;
+
+ private ElapsedTime runtime = new ElapsedTime();
+
+ // Calculate the COUNTS_PER_INCH for your specific drive train.
+ // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV
+ // For external drive gearing, set DRIVE_GEAR_REDUCTION as needed.
+ // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear.
+ // This is gearing DOWN for less speed and more torque.
+ // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation.
+ static final double COUNTS_PER_MOTOR_REV = 1440 ; // eg: TETRIX Motor Encoder
+ static final double DRIVE_GEAR_REDUCTION = 1.0 ; // No External Gearing.
+ static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference
+ static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
+ (WHEEL_DIAMETER_INCHES * 3.1415);
+ static final double DRIVE_SPEED = 0.6;
+ static final double TURN_SPEED = 0.5;
+
+ @Override
+ public void runOpMode() {
+
+ // Initialize the drive system variables.
+ 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. Gear Reduction or 90 Deg drives may require direction flips
+ leftDrive.setDirection(DcMotor.Direction.REVERSE);
+ rightDrive.setDirection(DcMotor.Direction.FORWARD);
+
+ leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+ leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+ // Send telemetry message to indicate successful Encoder reset
+ telemetry.addData("Starting at", "%7d :%7d",
+ leftDrive.getCurrentPosition(),
+ rightDrive.getCurrentPosition());
+ telemetry.update();
+
+ // Wait for the game to start (driver presses PLAY)
+ waitForStart();
+
+ // Step through each leg of the path,
+ // Note: Reverse movement is obtained by setting a negative distance (not speed)
+ encoderDrive(DRIVE_SPEED, 48, 48, 5.0); // S1: Forward 47 Inches with 5 Sec timeout
+ encoderDrive(TURN_SPEED, 12, -12, 4.0); // S2: Turn Right 12 Inches with 4 Sec timeout
+ encoderDrive(DRIVE_SPEED, -24, -24, 4.0); // S3: Reverse 24 Inches with 4 Sec timeout
+
+ telemetry.addData("Path", "Complete");
+ telemetry.update();
+ sleep(1000); // pause to display final telemetry message.
+ }
+
+ /*
+ * Method to perform a relative move, based on encoder counts.
+ * Encoders are not reset as the move is based on the current position.
+ * Move will stop if any of three conditions occur:
+ * 1) Move gets to the desired position
+ * 2) Move runs out of time
+ * 3) Driver stops the OpMode running.
+ */
+ public void encoderDrive(double speed,
+ double leftInches, double rightInches,
+ double timeoutS) {
+ int newLeftTarget;
+ int newRightTarget;
+
+ // Ensure that the OpMode is still active
+ if (opModeIsActive()) {
+
+ // Determine new target position, and pass to motor controller
+ newLeftTarget = leftDrive.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH);
+ newRightTarget = rightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);
+ leftDrive.setTargetPosition(newLeftTarget);
+ rightDrive.setTargetPosition(newRightTarget);
+
+ // Turn On RUN_TO_POSITION
+ leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+ rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+ // reset the timeout time and start motion.
+ runtime.reset();
+ leftDrive.setPower(Math.abs(speed));
+ rightDrive.setPower(Math.abs(speed));
+
+ // keep looping while we are still active, and there is time left, and both motors are running.
+ // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
+ // its target position, the motion will stop. This is "safer" in the event that the robot will
+ // always end the motion as soon as possible.
+ // However, if you require that BOTH motors have finished their moves before the robot continues
+ // onto the next step, use (isBusy() || isBusy()) in the loop test.
+ while (opModeIsActive() &&
+ (runtime.seconds() < timeoutS) &&
+ (leftDrive.isBusy() && rightDrive.isBusy())) {
+
+ // Display it for the driver.
+ telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget);
+ telemetry.addData("Currently at", " at %7d :%7d",
+ leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition());
+ telemetry.update();
+ }
+
+ // Stop all motion;
+ leftDrive.setPower(0);
+ rightDrive.setPower(0);
+
+ // Turn off RUN_TO_POSITION
+ leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+ sleep(250); // optional pause after each move.
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RobotAutoDriveByGyro_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RobotAutoDriveByGyro_Linear.java
new file mode 100644
index 0000000000..985a22164a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/RobotAutoDriveByGyro_Linear.java
@@ -0,0 +1,510 @@
+/* Copyright (c) 2022 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.teamcode.auto;
+
+import static com.acmerobotics.roadrunner.ftc.Actions.runBlocking;
+import static java.lang.Math.toRadians;
+
+import android.util.Size;
+
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.SequentialAction;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+import org.firstinspires.ftc.teamcode.MecanumDrive;
+import org.firstinspires.ftc.teamcode.vision.BlueDetectionLeft;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+/*
+ * This OpMode illustrates the concept of driving an autonomous path based on Gyro (IMU) heading and encoder counts.
+ * The code is structured as a LinearOpMode
+ *
+ * The path to be followed by the robot is built from a series of drive, turn or pause steps.
+ * Each step on the path is defined by a single function call, and these can be strung together in any order.
+ *
+ * The code REQUIRES that you have encoders on the drive motors, otherwise you should use: RobotAutoDriveByTime;
+ *
+ * This code uses the Universal IMU interface so it will work with either the BNO055, or BHI260 IMU.
+ * To run as written, the Control/Expansion hub should be mounted horizontally on a flat part of the robot chassis.
+ * The REV Logo should be facing UP, and the USB port should be facing forward.
+ * If this is not the configuration of your REV Control Hub, then the code should be modified to reflect the correct orientation.
+ *
+ * This sample requires that the drive Motors have been configured with names : left_drive and right_drive.
+ * It also requires that a positive power command moves both motors forward, and causes the encoders to count UP.
+ * So please verify that both of your motors move the robot forward on the first move. If not, make the required correction.
+ * See the beginning of runOpMode() to set the FORWARD/REVERSE option for each motor.
+ *
+ * This code uses RUN_TO_POSITION mode for driving straight, and RUN_USING_ENCODER mode for turning and holding.
+ * Note: This code implements the requirement of calling setTargetPosition() at least once before switching to RUN_TO_POSITION mode.
+ *
+ * Notes:
+ *
+ * All angles are referenced to the coordinate-frame that is set whenever resetHeading() is called.
+ * In this sample, the heading is reset when the Start button is touched on the Driver station.
+ * Note: It would be possible to reset the heading after each move, but this would accumulate steering errors.
+ *
+ * The angle of movement/rotation is assumed to be a standardized rotation around the robot Z axis,
+ * which means that a Positive rotation is Counter Clockwise, looking down on the field.
+ * This is consistent with the FTC field coordinate conventions set out in the document:
+ * https://ftc-docs.firstinspires.org/field-coordinate-system
+ *
+ * Control Approach.
+ *
+ * To reach, or maintain a required heading, this code implements a basic Proportional Controller where:
+ *
+ * Steering power = Heading Error * Proportional Gain.
+ *
+ * "Heading Error" is calculated by taking the difference between the desired heading and the actual heading,
+ * and then "normalizing" it by converting it to a value in the +/- 180 degree range.
+ *
+ * "Proportional Gain" is a constant that YOU choose to set the "strength" of the steering response.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your "TeamCode" folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ */
+
+@Autonomous(name="GyroEncoderAutoNoStrafe", group="Auton")
+public class RobotAutoDriveByGyro_Linear extends LinearOpMode {
+ private VisionPortal visionPortal;
+ private BlueDetectionLeft detector = new BlueDetectionLeft(telemetry);
+
+ public int color = 1;
+ /* Declare OpMode members. */
+ private DcMotorEx leftFront = null;
+ private DcMotorEx rightFront = null;
+ private DcMotorEx rightBack = null;
+ private DcMotorEx leftBack= null;
+ private IMU imu = null; // Control/Expansion Hub IMU
+
+ private double headingError = 0;
+
+ // These variable are declared here (as class members) so they can be updated in various methods,
+ // but still be displayed by sendTelemetry()
+ private double targetHeading = 0;
+ private double driveSpeed = 0;
+ private double turnSpeed = 0;
+ private double leftSpeed = 0;
+ private double rightSpeed = 0;
+ private int leftFrontTarget = 0;
+ private int rightFrontTarget = 0;
+
+ private int leftBackTarget = 0;
+ private int rightBackTarget = 0;
+
+ // Calculate the COUNTS_PER_INCH for your specific drive train.
+ // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV
+ // For external drive gearing, set DRIVE_GEAR_REDUCTION as needed.
+ // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear.
+ // This is gearing DOWN for less speed and more torque.
+ // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation.
+ static final double COUNTS_PER_MOTOR_REV = 28 ; // eg: GoBILDA 312 RPM Yellow Jacket
+ static final double DRIVE_GEAR_REDUCTION = 5 ; // No External Gearing.
+ static final double WHEEL_DIAMETER_INCHES = 2.953 ; // For figuring circumference
+ static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
+ (WHEEL_DIAMETER_INCHES * 3.1415);
+
+ // These constants define the desired driving/control characteristics
+ // They can/should be tweaked to suit the specific robot drive train.
+ static final double DRIVE_SPEED = 0.2; // Max driving speed for better distance accuracy.
+ static final double TURN_SPEED = 0.1; // Max Turn speed to limit turn rate
+ static final double HEADING_THRESHOLD = 1.0 ; // How close must the heading get to the target before moving to next step.
+ // Requiring more accuracy (a smaller number) will often make the turn take longer to get into the final position.
+ // Define the Proportional control coefficient (or GAIN) for "heading control".
+ // We define one value when Turning (larger errors), and the other is used when Driving straight (smaller errors).
+ // Increase these numbers if the heading does not corrects strongly enough (eg: a heavy robot or using tracks)
+ // Decrease these numbers if the heading does not settle on the correct value (eg: very agile robot with omni wheels)
+ static final double P_TURN_GAIN = 0.01; // Larger is more responsive, but also less stable
+ static final double P_DRIVE_GAIN = 0.02; // Larger is more responsive, but also less stable
+
+
+ @Override
+ public void runOpMode() {
+ MecanumDrive drive = new MecanumDrive(hardwareMap,new Pose2d(0, 0, 0));
+ // Initialize the drive system variables.
+ leftFront = drive.leftFront;
+ leftBack = drive.leftBack;
+ rightFront = drive.rightFront;
+ rightBack = drive.rightBack;
+
+
+ /* The next two lines define Hub orientation.
+ * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
+ *
+ * To Do: EDIT these two lines to match YOUR mounting configuration.
+ */
+ RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
+ RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.LEFT;
+ RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
+
+ // Now initialize the IMU with this mounting orientation
+ // This sample expects the IMU to be in a REV Hub and named "imu".
+ imu = hardwareMap.get(IMU.class, "imu");
+ imu.initialize(new IMU.Parameters(orientationOnRobot));
+
+ // Ensure the robot is stationary. Reset the encoders and set the motors to BRAKE mode
+ leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+ // Wait for the game to start (Display Gyro value while waiting)
+ while (opModeInInit()) {
+ telemetry.addData(">", "Robot Heading = %4.0f", getHeading());
+ telemetry.update();
+ initVision();
+ color = BlueDetectionLeft.getReadout();
+
+ telemetry.addData("Useless sees ",color);
+ telemetry.addData("LeftValue",BlueDetectionLeft.leftValue);
+ telemetry.addData("CenterValue",BlueDetectionLeft.centerValue);
+ telemetry.addData("RightValue",BlueDetectionLeft.rightValue);
+ telemetry.update();
+ //grab pixels here
+ }
+
+ // Set the encoders for closed loop speed control, and reset the heading.
+ leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ imu.resetYaw();
+
+ // Step through each leg of the path,
+ // Notes: Reverse movement is obtained by setting a negative distance (not speed)
+ // holdHeading() is used after turns to let the heading stabilize
+ // Add a sleep(2000) after any step to keep the telemetry data visible for review
+
+ if (color == 1) {
+ //do some
+
+ } else if (color ==2){
+
+
+ } else if (color == 3) {
+ sleep(1000);
+
+
+ } else {
+ //cant see shit
+ }
+ driveStraight(DRIVE_SPEED, 12.0, 0.0); // Drive Forward 24"
+ turnToHeading( TURN_SPEED, -45.0); // Turn CW to -45 Degrees
+ holdHeading( TURN_SPEED, -45.0, 1); // Hold -45 Deg heading for a 1/2 second
+ turnToHeading( TURN_SPEED, -90.0); // Turn CW to -45 Degrees
+ holdHeading( TURN_SPEED, -90.0, 1); // Hold -45 Deg heading for a 1/2 second
+
+ // release pixel here.
+
+// driveStraight(DRIVE_SPEED, 17.0, -45.0); // Drive Forward 17" at -45 degrees (12"x and 12"y)
+// turnToHeading( TURN_SPEED, 45.0); // Turn CCW to 45 Degrees
+// holdHeading( TURN_SPEED, 45.0, 0.5); // Hold 45 Deg heading for a 1/2 second
+//
+// driveStraight(DRIVE_SPEED, 17.0, 45.0); // Drive Forward 17" at 45 degrees (-12"x and 12"y)
+// turnToHeading( TURN_SPEED, 0.0); // Turn CW to 0 Degrees
+// holdHeading( TURN_SPEED, 0.0, 1.0); // Hold 0 Deg heading for 1 second
+//
+// driveStraight(DRIVE_SPEED,-48.0, 0.0); // Drive in Reverse 48" (should return to approx. staring position)
+
+ telemetry.addData("Path", "Complete");
+ telemetry.update();
+ sleep(1000); // Pause to display last telemetry message.
+ }
+
+ /*
+ * ====================================================================================================
+ * Driving "Helper" functions are below this line.
+ * These provide the high and low level methods that handle driving straight and turning.
+ * ====================================================================================================
+ */
+
+ // ********** HIGH Level driving functions. ********************
+
+ /**
+ * Drive in a straight line, on a fixed compass heading (angle), based on encoder counts.
+ * Move will stop if either of these conditions occur:
+ * 1) Move gets to the desired position
+ * 2) Driver stops the OpMode running.
+ *
+ * @param maxDriveSpeed MAX Speed for forward/rev motion (range 0 to +1.0) .
+ * @param distance Distance (in inches) to move from current position. Negative distance means move backward.
+ * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset.
+ * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+ * If a relative angle is required, add/subtract from the current robotHeading.
+ */
+ public void driveStraight(double maxDriveSpeed,
+ double distance,
+ double heading) {
+
+ // Ensure that the OpMode is still active
+ if (opModeIsActive()) {
+
+ // Determine new target position, and pass to motor controller
+ int moveCounts = (int)(distance * COUNTS_PER_INCH);
+ leftFrontTarget = leftFront.getCurrentPosition() + moveCounts;
+ rightFrontTarget = rightFront.getCurrentPosition() + moveCounts;
+ leftBackTarget = leftBack.getCurrentPosition() + moveCounts;
+ rightBackTarget = rightBack.getCurrentPosition() + moveCounts;
+
+
+ // Set Target FIRST, then turn on RUN_TO_POSITION
+ leftFront.setTargetPosition(leftFrontTarget);
+ rightFront.setTargetPosition(rightFrontTarget);
+ leftBack.setTargetPosition(leftFrontTarget);
+ rightBack.setTargetPosition(rightFrontTarget);
+
+ leftFront.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+ rightFront.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+ leftBack.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+ rightBack.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+ // Set the required driving speed (must be positive for RUN_TO_POSITION)
+ // Start driving straight, and then enter the control loop
+ maxDriveSpeed = Math.abs(maxDriveSpeed);
+ moveRobot(maxDriveSpeed, 0);
+
+ // keep looping while we are still active, and BOTH motors are running.
+ while (opModeIsActive() &&
+ (leftFront.isBusy() && rightFront.isBusy()&&leftBack.isBusy() && rightBack.isBusy())) {
+
+ // Determine required steering to keep on heading
+ turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN);
+
+ // if driving in reverse, the motor correction also needs to be reversed
+ if (distance < 0)
+ turnSpeed *= -1.0;
+
+ // Apply the turning correction to the current driving speed.
+ moveRobot(driveSpeed, turnSpeed);
+
+ // Display drive status for the driver.
+ sendTelemetry(true);
+ }
+
+ // Stop all motion & Turn off RUN_TO_POSITION
+ moveRobot(0, 0);
+ leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ }
+ }
+
+ /**
+ * Spin on the central axis to point in a new direction.
+ *
+ * Move will stop if either of these conditions occur:
+ *
+ * 1) Move gets to the heading (angle)
+ *
+ * 2) Driver stops the OpMode running.
+ *
+ * @param maxTurnSpeed Desired MAX speed of turn. (range 0 to +1.0)
+ * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset.
+ * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+ * If a relative angle is required, add/subtract from current heading.
+ */
+ public void turnToHeading(double maxTurnSpeed, double heading) {
+
+ // Run getSteeringCorrection() once to pre-calculate the current error
+ getSteeringCorrection(heading, P_DRIVE_GAIN);
+
+ // keep looping while we are still active, and not on heading.
+ while (opModeIsActive() && (Math.abs(headingError) > HEADING_THRESHOLD)) {
+
+ // Determine required steering to keep on heading
+ turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN);
+
+ // Clip the speed to the maximum permitted value.
+ turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed);
+
+ // Pivot in place by applying the turning correction
+ moveRobot(0, turnSpeed);
+
+ // Display drive status for the driver.
+ sendTelemetry(false);
+ }
+
+ // Stop all motion;
+ moveRobot(0, 0);
+ }
+
+ /**
+ * Obtain & hold a heading for a finite amount of time
+ *
+ * Move will stop once the requested time has elapsed
+ *
+ * This function is useful for giving the robot a moment to stabilize it's heading between movements.
+ *
+ * @param maxTurnSpeed Maximum differential turn speed (range 0 to +1.0)
+ * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset.
+ * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+ * If a relative angle is required, add/subtract from current heading.
+ * @param holdTime Length of time (in seconds) to hold the specified heading.
+ */
+ public void holdHeading(double maxTurnSpeed, double heading, double holdTime) {
+
+ ElapsedTime holdTimer = new ElapsedTime();
+ holdTimer.reset();
+
+ // keep looping while we have time remaining.
+ while (opModeIsActive() && (holdTimer.time() < holdTime)) {
+ // Determine required steering to keep on heading
+ turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN);
+
+ // Clip the speed to the maximum permitted value.
+ turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed);
+
+ // Pivot in place by applying the turning correction
+ moveRobot(0, turnSpeed);
+
+ // Display drive status for the driver.
+ sendTelemetry(false);
+ }
+
+ // Stop all motion;
+ moveRobot(0, 0);
+ }
+
+ // ********** LOW Level driving functions. ********************
+
+ /**
+ * Use a Proportional Controller to determine how much steering correction is required.
+ *
+ * @param desiredHeading The desired absolute heading (relative to last heading reset)
+ * @param proportionalGain Gain factor applied to heading error to obtain turning power.
+ * @return Turning power needed to get to required heading.
+ */
+ public double getSteeringCorrection(double desiredHeading, double proportionalGain) {
+ targetHeading = desiredHeading; // Save for telemetry
+
+ // Determine the heading current error
+ headingError = targetHeading - getHeading();
+
+ // Normalize the error to be within +/- 180 degrees
+ while (headingError > 180) headingError -= 360;
+ while (headingError <= -180) headingError += 360;
+
+ // Multiply the error by the gain to determine the required steering correction/ Limit the result to +/- 1.0
+ return Range.clip(headingError * proportionalGain, -1, 1);
+ }
+
+ /**
+ * Take separate drive (fwd/rev) and turn (right/left) requests,
+ * combines them, and applies the appropriate speed commands to the left and right wheel motors.
+ * @param drive forward motor speed
+ * @param turn clockwise turning motor speed.
+ */
+ public void moveRobot(double drive, double turn) {
+ driveSpeed = drive; // save this value as a class member so it can be used by telemetry.
+ turnSpeed = turn; // save this value as a class member so it can be used by telemetry.
+
+ leftSpeed = drive - turn;
+ rightSpeed = drive + turn;
+
+ // Scale speeds down if either one exceeds +/- 1.0;
+ double max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
+ if (max > 1.0)
+ {
+ leftSpeed /= max;
+ rightSpeed /= max;
+ }
+
+ leftFront.setPower(leftSpeed);
+ rightFront.setPower(rightSpeed);
+ leftBack.setPower(leftSpeed);
+ rightBack.setPower(rightSpeed);
+ }
+ private void initVision() {
+
+
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .addProcessors(detector)
+ .setCameraResolution(new Size(640,480))
+ .setStreamFormat(VisionPortal.StreamFormat.YUY2)
+ .enableLiveView(true)
+ .build();
+ }
+
+ /**
+ * Display the various control parameters while driving
+ *
+ * @param straight Set to true if we are driving straight, and the encoder positions should be included in the telemetry.
+ */
+ private void sendTelemetry(boolean straight) {
+
+ if (straight) {
+ telemetry.addData("Motion", "Drive Straight");
+ telemetry.addData("Target Pos L:R", "%7d:%7d", leftFrontTarget, rightFrontTarget);
+ telemetry.addData("Actual Pos L:R", "%7d:%7d", leftFront.getCurrentPosition(),
+
+ rightFront.getCurrentPosition());
+ telemetry.addData("Target Pos L:R", "%7d:%7d", leftBackTarget, rightBackTarget);
+ telemetry.addData("Actual Pos L:R", "%7d:%7d", leftBack.getCurrentPosition(), rightBack.getCurrentPosition());
+ } else {
+ telemetry.addData("Motion", "Turning");
+ }
+
+ telemetry.addData("Heading- Target : Current", "%5.2f : %5.0f", targetHeading, getHeading());
+ telemetry.addData("Error : Steer Pwr", "%5.1f : %5.1f", headingError, turnSpeed);
+ telemetry.addData("Wheel Speeds LF : RF", "%5.2f : %5.2f", leftSpeed, rightSpeed);
+ telemetry.update();
+ }
+
+ /**
+ * read the Robot heading directly from the IMU (in degrees)
+ */
+ public double getHeading() {
+ YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
+ return orientation.getYaw(AngleUnit.DEGREES);
+ }
+
+ @Override
+ public void waitForStart() {
+ super.waitForStart();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOP.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOP.java
index 9046257aab..d8fb315bf0 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOP.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleOP.java
@@ -21,25 +21,31 @@ public class TeleOP extends LinearOpMode {
double claw1Grab = 0.51;
double claw2Grab = 0.28;
double wrist1Pos = 0.2;
- double wrist2Pos = 0.4;
+ double wrist2Pos = 1.0;
double arm1Pos = 0.3;
double arm2Pos = 0.7;
//-------------------------------------------
//other stuff idk
- Gamepad currentGamepad1, previousGamepad1;
- Gamepad currentGamepad2, previousGamepad2;
+ Gamepad currentGamepad1 = new Gamepad();
+ Gamepad previousGamepad1 = new Gamepad();
+
@Override
public void runOpMode() throws InterruptedException {
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
-
+ drive.imu.resetYaw();
waitForStart();
double straight, turn, strafe;
double liftPower, claw1Pos, claw2Pos, wristPos, armPos;
+ armPos = 0;
+ wristPos = 0;
+ claw1Pos = 0;
+ claw2Pos = 0;
+
int transferState = 0;
int intakeState = 0;
@@ -47,8 +53,8 @@ public void runOpMode() throws InterruptedException {
boolean intake1 = false;
while (opModeIsActive()) {
- currentGamepad1 = gamepad1;
- previousGamepad1 = currentGamepad1;
+ currentGamepad1.copy(gamepad1);
+
straight = (-currentGamepad1.left_stick_y > 0.05) ? (Math.pow(-gamepad1.left_stick_y, 3) + 0.3) : (-gamepad1.left_stick_y < -0.05) ? (Math.pow(-gamepad1.left_stick_y, 3) - 0.3) : 0;
strafe = (gamepad1.left_stick_x > 0.05) ? (Math.pow(gamepad1.left_stick_x, 3) + 0.3) : (gamepad1.left_stick_x < -0.05) ? (Math.pow(gamepad1.left_stick_x, 3) - 0.3) : 0;
@@ -63,88 +69,78 @@ public void runOpMode() throws InterruptedException {
//IMU Feedback data------------------------------------------------------------
YawPitchRollAngles angles = drive.imu.getRobotYawPitchRollAngles();
- telemetry.addData("Pitch",angles.getPitch(AngleUnit.DEGREES));
- telemetry.addData("Roll",angles.getRoll(AngleUnit.DEGREES));
telemetry.addData("Yaw",angles.getYaw(AngleUnit.DEGREES));
//------------------------------------------------------------------------------
//INTAKE LMAOOOOOOO ------------------------------------------
- if (gamepad1.right_bumper && intakeState == 0){
- claw1Pos = claw1Grab;
- intakeState = 1;
- }
- else if (gamepad1.right_bumper && intakeState == 1){
- claw2Pos = claw2Grab;
- intakeState = 2;
+ if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper){
+ intakeState = (intakeState +1)%5;
}
- else if (gamepad1.right_bumper && intakeState == 2){
- claw2Pos = 0;
- intakeState = 3;
+ if (currentGamepad1.x && !previousGamepad1.x) {
+ transferState = (transferState + 1)%3;
}
- else if (gamepad1.right_bumper && intakeState == 3){
- claw1Pos = 0;
- armPos = 0;
- wristPos = 0;
- intakeState = 0;
- transferState = 0;
- }
- //------------------------------------------------------------
-
- //TRANSFER TSTUFFFFFFFFF ------------------------------------------
- if (gamepad1.x && transferState == 0){
- //drop front
- armPos = arm1Pos;
- wristPos = wrist1Pos;
- transferState = 1;
- }
- else if (gamepad1.x && transferState == 1){
- //drop back
- armPos = arm2Pos;
- wristPos = wrist2Pos;
- transferState = 2;
+
+ switch (intakeState){
+ case 0:
+ claw1Pos= 0;
+ claw2Pos = 0;
+ break;
+ case 1:
+ claw1Pos = claw1Grab;
+ claw2Pos = claw2Grab;
+ break;
+ case 2:
+ claw2Pos = 0;
+ break;
+ case 3:
+ claw1Pos = 0;
+ break;
+ case 4:
+ armPos = 0;
+ wristPos = 0;
+ transferState = 0;
+ intakeState = 0;
+ break;
}
- else if (gamepad1.x && transferState == 2){
- //reset
- armPos = 0;
- wristPos = 0;
- transferState = 0;
+
+ if (gamepad1.dpad_left){drive.imu.resetYaw();}
+
+ //TRANSFER TSTUFFFFFFFFF ----------------------------------------
+
+ switch (transferState){
+ case 0:
+ armPos = 0;
+ wristPos = 0;
+ break;
+ case 1:
+ armPos = arm1Pos;
+ wristPos = wrist1Pos;
+ break;
+ case 2:
+ armPos = arm2Pos;
+ wristPos = wrist2Pos;
+ break;
}
+
+
//------------------------------------------------------------------
//LIFT LIFT ------------------------------------------------------------
- else if (gamepad1.left_trigger >= 0.1){
+ if (gamepad1.left_trigger >= 0.1){
//lift go up
- drive.lift1.setPower(gamepad1.left_trigger);
- drive.lift2.setPower(-gamepad1.left_trigger);
+ liftPower = gamepad1.left_trigger/2;
}
else if (gamepad1.left_bumper){
//lift go down
- drive.lift1.setPower(-0.4);
- drive.lift2.setPower(0.4);
- }
+ liftPower = -0.2; }
else{
- //lift hold
- drive.lift1.setPower(0.05);
- drive.lift2.setPower(-0.05);
+ liftPower = 0;
}
//------------------------------------------------------------------
-
-
- //=---------------------------------------------
- claw1Pos = gamepad2.left_stick_x;
- claw2Pos = gamepad2.right_stick_x;
- //0.28
- //0.51
-
- armPos = currentGamepad1.left_trigger;
- //0.7 all the way back, 0.05 off the ground a bit, assuming 0.3 for front placement.
- wristPos = currentGamepad1.right_trigger;
- //1.0 max
- //------------------------------------------------
-
-
+ telemetry.addData("TransState",transferState);
+ telemetry.addData("INTState",intakeState);
telemetry.addData("armPos",armPos);
telemetry.addData("wristPos",wristPos);
telemetry.addData("claw1Pos",claw1Pos);
@@ -155,7 +151,10 @@ else if (gamepad1.left_bumper){
drive.wrist.setPosition(wristPos);
drive.arm1.setPosition(armPos);
drive.arm2.setPosition(armPos);
+ drive.lift1.setPower(liftPower);
+ drive.lift2.setPower(liftPower);
telemetry.update();
+ previousGamepad1.copy(currentGamepad1);
}
}
-}
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/BlueDetectionLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/BlueDetectionLeft.java
new file mode 100644
index 0000000000..8e1d07b5ec
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/BlueDetectionLeft.java
@@ -0,0 +1,127 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+
+import android.graphics.Canvas;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
+import org.firstinspires.ftc.vision.VisionProcessor;
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
+public class BlueDetectionLeft implements VisionProcessor {
+ Telemetry telemetry;
+ Mat mat = new Mat();
+
+ Scalar lowBlueHSV = new Scalar(90, 50, 50);
+ Scalar highBlueHSV = new Scalar(130, 255, 255);
+
+// Scalar lower_red1 = new Scalar(0,50,50);
+// Scalar upper_red1 = new Scalar(10,255,255);
+// Scalar lower_red2 = new Scalar(160,50,50);
+// Scalar upper_red2 = new Scalar(180,255,255);
+
+ static int readout = 0;
+
+ public static double leftValue;
+ public static double rightValue;
+ public static double centerValue;
+ public static double leftPixels;
+ public static double rightPixels;
+ public static double centerPixels;
+
+ public static double thresh = 0.06;
+
+
+ static final Rect LEFT_ROI = new Rect(
+ new Point(0, 120),
+ new Point(100, 320));
+ static final Rect CENTER_ROI = new Rect(
+ new Point(140,120),
+ new Point(140+300,120+130));
+ static final Rect RIGHT_ROI = new Rect(
+ new Point(500, 120),
+ new Point(640, 320));
+
+ Scalar No = new Scalar(255, 0, 0);
+ Scalar Yes = new Scalar(0, 255, 0);
+
+
+ public BlueDetectionLeft(Telemetry telemetry) {
+ this.telemetry = telemetry;
+ }
+
+ @Override
+ public void init(int width, int height, CameraCalibration calibration) {
+ }
+
+
+
+ @Override
+ public Object processFrame(Mat frame, long captureTimeNanos) {
+ Imgproc.blur(frame,frame, new Size(4,4));
+
+ Imgproc.cvtColor(frame, mat, Imgproc.COLOR_RGB2HSV);
+
+
+ Core.inRange(mat, lowBlueHSV, highBlueHSV, mat);
+// Core.inRange(mat, lower_red1,upper_red1,mat);
+// Core.inRange(mat, lower_red2,upper_red2,matExtra);
+
+// Core.bitwise_or(mat,matExtra,mat);
+
+
+ Mat left = mat.submat(LEFT_ROI);
+ Mat center = mat.submat(CENTER_ROI);
+ Mat right = mat.submat(RIGHT_ROI);
+
+ leftPixels = Core.countNonZero(left);
+ rightPixels = Core.countNonZero(right);
+ centerPixels = Core.countNonZero(center);
+
+ leftValue = leftPixels / LEFT_ROI.area();
+ centerValue = centerPixels / CENTER_ROI.area();
+ rightValue = rightPixels / RIGHT_ROI.area();
+
+ left.release();
+ center.release();
+ right.release();
+
+ if (leftPixels>rightPixels && leftPixels>centerPixels&&(leftValue>thresh)){
+ readout = 1;
+ }else if (centerPixels>rightPixels && centerPixels>leftPixels&&(centerValue>thresh)){
+ readout = 2;
+ }else if (rightPixels>leftPixels && rightPixels>centerPixels&&(rightValue>thresh)){
+ readout = 3;
+ }else{
+ readout = 0;
+ }
+
+ Imgproc.cvtColor(mat, mat, Imgproc.COLOR_GRAY2RGB);
+
+ Imgproc.rectangle(mat, LEFT_ROI, readout == 1? Yes:No);
+ Imgproc.rectangle(mat, CENTER_ROI, readout == 2? Yes:No);
+ Imgproc.rectangle(mat, RIGHT_ROI, readout == 3? Yes:No);
+
+ mat.copyTo(frame);
+ mat.release();
+ return null;
+ }
+
+ public static int getReadout(){
+ return readout;
+ }
+
+ @Override
+ public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) {
+ // Not useful either
+ }
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/DualPortals.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/DualPortals.java
new file mode 100644
index 0000000000..375971ad24
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/DualPortals.java
@@ -0,0 +1,217 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+import android.util.Size;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.sun.tools.javac.util.List;
+
+import org.firstinspires.ftc.robotcore.external.JavaUtil;
+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.ArrayList;
+
+@TeleOp(name = "W_MultiPortal_v01 (Blocks to Java)")
+public class DualPortals extends LinearOpMode {
+
+ VisionPortal.Builder myVisionPortalBuilder;
+ boolean USE_WEBCAM_1;
+ int Portal_1_View_ID;
+ boolean USE_WEBCAM_2;
+ int Portal_2_View_ID;
+ AprilTagProcessor myAprilTagProcessor_1;
+ AprilTagProcessor myAprilTagProcessor_2;
+ VisionPortal myVisionPortal_1;
+ VisionPortal myVisionPortal_2;
+
+ /**
+ * Describe this function...
+ */
+ private void initMultiPortals() {
+ List myPortalsList;
+
+ myPortalsList = (List) JavaUtil.makeIntegerList(VisionPortal.makeMultiPortalView(2, VisionPortal.MultiPortalLayout.HORIZONTAL));
+ Portal_1_View_ID = ((Integer) JavaUtil.inListGet(myPortalsList, JavaUtil.AtMode.FROM_START, 0, false)).intValue();
+ Portal_2_View_ID = ((Integer) JavaUtil.inListGet(myPortalsList, JavaUtil.AtMode.FROM_START, 1, false)).intValue();
+ telemetry.addData("Portal 1 View ID (index 0 of myPortalsList)", Portal_1_View_ID);
+ telemetry.addData("Portal 2 View ID (index 1 of myPortalsList)", Portal_2_View_ID);
+ telemetry.addLine("");
+ telemetry.addLine("Press Y to continue");
+ telemetry.update();
+ while (!gamepad1.y && opModeInInit()) {
+ // Loop until gamepad Y button is pressed.
+ }
+ }
+
+ /**
+ * This Op Mode demonstrates MultiPortalView.
+ *
+ * The Dpad buttons can turn each camera stream on and off.
+ * USB bandwidth is more restricted on an external USB hub, compared to the Control Hub.
+ */
+ @Override
+ public void runOpMode() {
+ // This OpMode shows AprilTag recognition and pose estimation.
+ USE_WEBCAM_1 = true;
+ USE_WEBCAM_2 = true;
+ initMultiPortals();
+ // Initialize AprilTag before waitForStart.
+ initAprilTag();
+ // Wait for the Start button to be touched.
+ telemetry.addData("DS preview on/off", "3 dots, Camera Stream");
+ telemetry.addData(">", "Touch Play to start OpMode");
+ telemetry.update();
+ waitForStart();
+ if (opModeIsActive()) {
+ while (opModeIsActive()) {
+ AprilTag_telemetry_for_Portal_1();
+ AprilTag_telemetry_for_Portal_2();
+ AprilTag_telemetry_legend();
+ Toggle_camera_streams();
+ // Push telemetry to the Driver Station.
+ telemetry.update();
+ // Share the CPU.
+ sleep(20);
+ }
+ }
+ }
+
+ /**
+ * Initialize AprilTag Detection.
+ */
+ private void initAprilTag() {
+ AprilTagProcessor.Builder myAprilTagProcessorBuilder;
+
+ // First, create an AprilTagProcessor.Builder.
+ myAprilTagProcessorBuilder = new AprilTagProcessor.Builder();
+ // Create each AprilTagProcessor by calling build.
+ myAprilTagProcessor_1 = myAprilTagProcessorBuilder.build();
+ myAprilTagProcessor_2 = myAprilTagProcessorBuilder.build();
+ Make_first_VisionPortal();
+ Make_second_VisionPortal();
+ }
+
+ /**
+ * Describe this function...
+ */
+ private void Make_first_VisionPortal() {
+ // Create a VisionPortal.Builder and set attributes related to the first camera.
+ myVisionPortalBuilder = new VisionPortal.Builder();
+ myVisionPortalBuilder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"));
+ myVisionPortalBuilder.setCameraResolution(new Size(640, 480));
+ // Manage USB bandwidth of two camera streams, by selecting Streaming Format.
+ // Set the stream format.
+ myVisionPortalBuilder.setStreamFormat(VisionPortal.StreamFormat.MJPEG);
+ // Add myAprilTagProcessor to the VisionPortal.Builder.
+ myVisionPortalBuilder.addProcessor(myAprilTagProcessor_1);
+ // Add the Portal View ID to the VisionPortal.Builder
+ // Set the camera monitor view id.
+ myVisionPortalBuilder.setLiveViewContainerId(Portal_1_View_ID);
+ // Create a VisionPortal by calling build.
+ myVisionPortal_1 = myVisionPortalBuilder.build();
+ }
+
+ /**
+ * Describe this function...
+ */
+ private void Make_second_VisionPortal() {
+ myVisionPortalBuilder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 2"));
+ myVisionPortalBuilder.setCameraResolution(new Size(640, 480));
+ myVisionPortalBuilder.setStreamFormat(VisionPortal.StreamFormat.MJPEG);
+ myVisionPortalBuilder.addProcessor(myAprilTagProcessor_2);
+ myVisionPortalBuilder.setLiveViewContainerId(Portal_2_View_ID);
+ myVisionPortal_2 = myVisionPortalBuilder.build();
+ }
+
+ /**
+ * Describe this function...
+ */
+ private void Toggle_camera_streams() {
+ // Manage USB bandwidth of two camera streams, by turning on or off.
+ if (gamepad1.dpad_down) {
+ // Temporarily stop the streaming session. This can save CPU
+ // resources, with the ability to resume quickly when needed.
+ myVisionPortal_1.stopStreaming();
+ } else if (gamepad1.dpad_up) {
+ // Resume the streaming session if previously stopped.
+ myVisionPortal_1.resumeStreaming();
+ }
+ if (gamepad1.dpad_left) {
+ // Temporarily stop the streaming session. This can save CPU
+ // resources, with the ability to resume quickly when needed.
+ myVisionPortal_2.stopStreaming();
+ } else if (gamepad1.dpad_right) {
+ // Resume the streaming session if previously stopped.
+ myVisionPortal_2.resumeStreaming();
+ }
+ }
+
+ /**
+ * Display info (using telemetry) for a recognized AprilTag.
+ */
+ private void AprilTag_telemetry_for_Portal_1() {
+ ArrayList myAprilTagDetections_1;
+ AprilTagDetection thisDetection_1;
+
+ // Get a list of AprilTag detections.
+ myAprilTagDetections_1 = myAprilTagProcessor_1.getDetections();
+ telemetry.addData("Portal 1 - # AprilTags Detected", JavaUtil.listLength(myAprilTagDetections_1));
+ // Iterate through list and call a function to display info for each recognized AprilTag.
+ for (AprilTagDetection thisDetection_1_item : myAprilTagDetections_1) {
+ thisDetection_1 = thisDetection_1_item;
+ // Display info about the detection.
+ telemetry.addLine("");
+ if (thisDetection_1.metadata != null) {
+ telemetry.addLine("==== (ID " + thisDetection_1.id + ") " + thisDetection_1.metadata.name);
+ telemetry.addLine("XYZ " + JavaUtil.formatNumber(thisDetection_1.ftcPose.x, 6, 1) + " " + JavaUtil.formatNumber(thisDetection_1.ftcPose.y, 6, 1) + " " + JavaUtil.formatNumber(thisDetection_1.ftcPose.z, 6, 1) + " (inch)");
+ telemetry.addLine("PRY " + JavaUtil.formatNumber(thisDetection_1.ftcPose.yaw, 6, 1) + " " + JavaUtil.formatNumber(thisDetection_1.ftcPose.pitch, 6, 1) + " " + JavaUtil.formatNumber(thisDetection_1.ftcPose.roll, 6, 1) + " (deg)");
+ telemetry.addLine("RBE " + JavaUtil.formatNumber(thisDetection_1.ftcPose.range, 6, 1) + " " + JavaUtil.formatNumber(thisDetection_1.ftcPose.bearing, 6, 1) + " " + JavaUtil.formatNumber(thisDetection_1.ftcPose.elevation, 6, 1) + " (inch, deg, deg)");
+ } else {
+ telemetry.addLine("==== (ID " + thisDetection_1.id + ") Unknown");
+ telemetry.addLine("Center " + JavaUtil.formatNumber(thisDetection_1.center.x, 6, 0) + "" + JavaUtil.formatNumber(thisDetection_1.center.y, 6, 0) + " (pixels)");
+ }
+ }
+ }
+
+ /**
+ * Display info (using telemetry) for a recognized AprilTag.
+ */
+ private void AprilTag_telemetry_for_Portal_2() {
+ ArrayList myAprilTagDetections_2;
+ AprilTagDetection thisDetection_2;
+
+ // Get a list of AprilTag detections.
+ myAprilTagDetections_2 = myAprilTagProcessor_2.getDetections();
+ telemetry.addLine("");
+ telemetry.addData("Portal 2 - # AprilTags Detected", JavaUtil.listLength(myAprilTagDetections_2));
+ // Iterate through list and call a function to display info for each recognized AprilTag.
+ for (AprilTagDetection thisDetection_2_item : myAprilTagDetections_2) {
+ thisDetection_2 = thisDetection_2_item;
+ // Display info about the detection.
+ telemetry.addLine("");
+ if (thisDetection_2.metadata != null) {
+ telemetry.addLine("==== (ID " + thisDetection_2.id + ") " + thisDetection_2.metadata.name);
+ telemetry.addLine("XYZ " + JavaUtil.formatNumber(thisDetection_2.ftcPose.x, 6, 1) + " " + JavaUtil.formatNumber(thisDetection_2.ftcPose.y, 6, 1) + " " + JavaUtil.formatNumber(thisDetection_2.ftcPose.z, 6, 1) + " (inch)");
+ telemetry.addLine("PRY " + JavaUtil.formatNumber(thisDetection_2.ftcPose.yaw, 6, 1) + " " + JavaUtil.formatNumber(thisDetection_2.ftcPose.pitch, 6, 1) + " " + JavaUtil.formatNumber(thisDetection_2.ftcPose.roll, 6, 1) + " (deg)");
+ telemetry.addLine("RBE " + JavaUtil.formatNumber(thisDetection_2.ftcPose.range, 6, 1) + " " + JavaUtil.formatNumber(thisDetection_2.ftcPose.bearing, 6, 1) + " " + JavaUtil.formatNumber(thisDetection_2.ftcPose.elevation, 6, 1) + " (inch, deg, deg)");
+ } else {
+ telemetry.addLine("==== (ID " + thisDetection_2.id + ") Unknown");
+ telemetry.addLine("Center " + JavaUtil.formatNumber(thisDetection_2.center.x, 6, 0) + "" + JavaUtil.formatNumber(thisDetection_2.center.y, 6, 0) + " (pixels)");
+ }
+ }
+ }
+
+ /**
+ * Describe this function...
+ */
+ private void AprilTag_telemetry_legend() {
+ telemetry.addLine("");
+ telemetry.addLine("key:");
+ telemetry.addLine("XYZ = X (Right), Y (Forward), Z (Up) dist.");
+ telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)");
+ telemetry.addLine("RBE = Range, Bearing & Elevation");
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/RedDetectionRight.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/RedDetectionRight.java
new file mode 100644
index 0000000000..fc642ea262
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/RedDetectionRight.java
@@ -0,0 +1,123 @@
+package org.firstinspires.ftc.teamcode.vision;
+
+
+import android.graphics.Canvas;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
+import org.firstinspires.ftc.vision.VisionProcessor;
+import org.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Rect;
+import org.opencv.core.Scalar;
+import org.opencv.core.Size;
+import org.opencv.imgproc.Imgproc;
+
+public class RedDetectionRight implements VisionProcessor {
+ Telemetry telemetry;
+ Mat mat = new Mat();
+ Mat matExtra = new Mat();
+
+// Scalar lowBlueHSV = new Scalar(90, 50, 50);
+// Scalar highBlueHSV = new Scalar(130, 255, 255);
+
+ Scalar lower_red1 = new Scalar(0,50,50);
+ Scalar upper_red1 = new Scalar(10,255,255);
+ Scalar lower_red2 = new Scalar(160,50,50);
+ Scalar upper_red2 = new Scalar(180,255,255);
+
+ static int readout = 0;
+
+ public static double leftValue;
+ public static double rightValue;
+ public static double centerValue;
+ public static double leftPixels;
+ public static double rightPixels;
+ public static double centerPixels;
+
+ public static double thresh = 0.05;
+
+ static final Rect LEFT_ROI = new Rect(
+ new Point(0, 120),
+ new Point(100, 320));
+ static final Rect CENTER_ROI = new Rect(
+ new Point(140,120),
+ new Point(140+300,120+130));
+ static final Rect RIGHT_ROI = new Rect(
+ new Point(500, 120),
+ new Point(640, 320));
+
+ Scalar No = new Scalar(255, 0, 0);
+ Scalar Yes = new Scalar(0, 255, 0);
+
+
+ public RedDetectionRight(Telemetry telemetry) {
+ this.telemetry = telemetry;
+ }
+
+ @Override
+ public void init(int width, int height, CameraCalibration calibration) {
+ }
+
+
+
+ @Override
+ public Object processFrame(Mat frame, long captureTimeNanos) {
+ Imgproc.blur(frame,frame, new Size(4,4));
+ Imgproc.cvtColor(frame, mat, Imgproc.COLOR_RGB2HSV);
+
+// Core.inRange(mat, lowBlueHSV, highBlueHSV, mat);
+ Core.inRange(mat, lower_red1,upper_red1,mat);
+ Core.inRange(mat, lower_red2,upper_red2,matExtra);
+
+ Core.bitwise_or(mat,matExtra,mat);
+
+
+ Mat left = mat.submat(LEFT_ROI);
+ Mat center = mat.submat(CENTER_ROI);
+ Mat right = mat.submat(RIGHT_ROI);
+
+ leftPixels = Core.countNonZero(left);
+ rightPixels = Core.countNonZero(right);
+ centerPixels = Core.countNonZero(center);
+
+ leftValue = leftPixels / LEFT_ROI.area();
+ centerValue = centerPixels / CENTER_ROI.area();
+ rightValue = rightPixels / RIGHT_ROI.area();
+
+ left.release();
+ center.release();
+ right.release();
+
+ if (leftPixels>rightPixels && leftPixels>centerPixels&&(leftValue>thresh)){
+ readout = 1;
+ }else if (centerPixels>rightPixels && centerPixels>leftPixels&&(centerValue>thresh)){
+ readout = 2;
+ }else if (rightPixels>leftPixels && rightPixels>centerPixels&&(rightValue>thresh)){
+ readout = 3;
+ }else{
+ readout = 0;
+ }
+
+ Imgproc.cvtColor(mat, mat, Imgproc.COLOR_GRAY2RGB);
+
+ Imgproc.rectangle(mat, LEFT_ROI, readout == 1? Yes:No);
+ Imgproc.rectangle(mat, CENTER_ROI, readout == 2? Yes:No);
+ Imgproc.rectangle(mat, RIGHT_ROI, readout == 3? Yes:No);
+
+ mat.copyTo(frame);
+ mat.release();
+ return null;
+ }
+
+ public static int getReadout(){
+ return readout;
+ }
+
+ @Override
+ public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) {
+ // Not useful either
+ }
+
+}
\ No newline at end of file