code

package org.firstinspires.ftc.teamcode;
 
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import java.util.List;
import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
 
/**
 * This 2020-2021 OpMode illustrates the basics of using the TensorFlow Object Detection API to
 * determine the position of the Ultimate Goal game elements.
 *
 * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
 * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list.
 *
 * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
 * is explained below.
 */
@Autonomous(name = "Ff", group = "Concept")
 
public class Ff extends LinearOpMode {
    private static final String TFOD_MODEL_ASSET = "UltimateGoal.tflite";
    private static final String LABEL_FIRST_ELEMENT = "Quad";
    private static final String LABEL_SECOND_ELEMENT = "Single";
    private DcMotor leftDrive2 = null;
    private DcMotor rightDrive = null;
    private DcMotor rightDrive2 = null;
    private DcMotor leftDrive = null;
    private DcMotor Intake = null;
   // private DcMotor test = null;
   private Servo servo0 = null;
 
   private DcMotor Conveyor = null;
   //private Servo Stopper = null;
   private DcMotor Launch1 = null;
   private Servo servo1 = null;
   private Servo servo2 = null;
   private Servo servo3 = null;
   private Servo right = null;
   private Servo left = null;
   private Servo armvert = null;
   private Servo armclose = null;
   private Servo ringclose = null;
   private Servo trigger = null;
   private DcMotor WobbleArm = null;
 
    /*
     * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
     * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
     * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
     * web site at https://developer.vuforia.com/license-manager.
     *
     * Vuforia license keys are always 380 characters long, and look as if they contain mostly
     * random data. As an example, here is a example of a fragment of a valid key:
     *      ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
     * Once you've obtained a license key, copy the string from the Vuforia web site
     * and paste it in to your code on the next line, between the double quotes.
     */
    private static final String VUFORIA_KEY =
            "";
 
    /**
     * {@link #vuforia} is the variable we will use to store our instance of the Vuforia
     * localization engine.
     */
    private VuforiaLocalizer vuforia;
 
    /**
     * {@link #tfod} is the variable we will use to store our instance of the TensorFlow Object
     * Detection engine.
     */
    private TFObjectDetector tfod;
 
    @Override
    public void runOpMode() {
        // The TFObjectDetector uses the camera frames from the VuforiaLocalizer, so we create that
        // first.
        initVuforia();
        initTfod();
 
        leftDrive2  = hardwareMap.get(DcMotor.class, "left_drive2");
        rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
        rightDrive2 = hardwareMap.get(DcMotor.class, "right_drive2");
        leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
        Intake = hardwareMap.get(DcMotor.class, "Intake");
        //test = hardwareMap.get(DcMotor.class, "test");
        servo0 = hardwareMap.get(Servo.class, "servo0");
 
        Conveyor = hardwareMap.get(DcMotor.class, "Conveyor");
        Launch1 = hardwareMap.get(DcMotor.class, "Launch1");
      //  Stopper = hardwareMap.get(Servo.class, "Stopper");
        servo1 = hardwareMap.get(Servo.class, "servo1");
        servo2 = hardwareMap.get(Servo.class, "servo2");
        servo3 = hardwareMap.get(Servo.class, "servo3");
        right = hardwareMap.get(Servo.class, "right");
        left = hardwareMap.get(Servo.class, "left");
        armvert = hardwareMap.get(Servo.class, "armvert");
        ringclose = hardwareMap.get(Servo.class, "ringclose");
        armclose = hardwareMap.get(Servo.class, "armclose");
        trigger = hardwareMap.get(Servo.class, "trigger");
         WobbleArm = hardwareMap.get(DcMotor.class, "WobbleArm");
 
 
        /**
         * Activate TensorFlow Object Detection before we wait for the start command.
         * Do it here so that the Camera Stream window will have the TensorFlow annotations visible.
         **/
        if (tfod != null) {
            tfod.activate();
 
            // The TensorFlow software will scale the input images from the camera to a lower resolution.
            // This can result in lower detection accuracy at longer distances (> 55cm or 22").
            // If your target is at distance greater than 50 cm (20") you can adjust the magnification value
            // to artificially zoom in to the center of image.  For best results, the "aspectRatio" argument
            // should be set to the value of the images used to create the TensorFlow Object Detection model
            // (typically 1.78 or 16/9).
 
            // Uncomment the following line if you want to adjust the magnification and/or the aspect ratio of the input images.
            //tfod.setZoom(2.5, 1.78);
        }
 
        /** Wait for the game to begin */
 
        telemetry.addData(">", "PLEASE WAIT 5 SECONDS BEFORE RUNNING");
        telemetry.update();
        waitForStart();
 
        if (opModeIsActive()) {
            while (opModeIsActive()) {
 
 
 
                if (tfod != null) {
                    // getUpdatedRecognitions() will return null if no new information is available since
                    // the last time that call was made.
                    List<Recognition> updatedRecognitions = tfod.getUpdatedRecognitions();
                    if (updatedRecognitions != null) {
                      telemetry.addData("# Object Detected", updatedRecognitions.size());
 
 
 
 
 
                       { 
                           if (updatedRecognitions.size() == 0){
 
 
                        //no rings 
 
                        telemetry.addData("TFOD", "Not Items Detected" );
                        telemetry.addData("Target Zone", "A"); 
                        telemetry.update();
 
 
                          telemetry.update();
                   // WobbleArm.setPower(-1);
                    //ringclose.setPosition(1);
 
                    sleep(1000);
 
                    //armvert.setPosition(-1);
                    ringclose.setPosition(1);
                    leftDrive2.setPower(-0.5) ;
                    rightDrive.setPower(0.5) ;
                    leftDrive.setPower(0.5) ;
                    rightDrive2.setPower(-0.5) ;
 
                    sleep(3300);
 
                    leftDrive2.setPower(0) ;
                    rightDrive.setPower(0) ;
                    leftDrive.setPower(0) ;
                    rightDrive2.setPower(0) ;
 
                    sleep(500);
 
                   WobbleArm.setPower(0.7);
 
                    sleep(700);
 
                    ringclose.setPosition(-1);
 
                    sleep(1000);
 
                    WobbleArm.setPower(-0.8);
                    sleep(200);
                    ringclose.setPosition(-1);
 
                    sleep(1000);
                    ringclose.setPosition(-1);
                    leftDrive2.setPower(0.5) ;
                    rightDrive.setPower(-0.5) ;
                    leftDrive.setPower(-0.5) ;
                    rightDrive2.setPower(0.5) ;
                    sleep(3650);
 
                    leftDrive2.setPower(0.5) ;
                    rightDrive.setPower(0.5) ;
                    leftDrive.setPower(-0.5) ;
                    rightDrive2.setPower(-0.5) ;
                    sleep(1750);
                    leftDrive2.setPower(0) ;
                    rightDrive.setPower(0) ;
                    leftDrive.setPower(0) ;
                    rightDrive2.setPower(0) ;
                    sleep(100);
                 /*   WobbleArm.setPower(0.4);
                    sleep(900);
                    ringclose.setPosition(1);
                    sleep(700);
                    WobbleArm.setPower(-0.8);
                    *///sleep(100);
                    leftDrive2.setPower(-0.8) ;
                    rightDrive.setPower(0.8) ;
                    leftDrive.setPower(0.8) ;
                    rightDrive2.setPower(-0.8) ;
                    sleep(1900);
                 /*   leftDrive2.setPower(1) ;
                    rightDrive.setPower(1) ;
                    leftDrive.setPower(-1) ;
                    rightDrive2.setPower(-1) ;
                    */
                    sleep(700);
                    leftDrive2.setPower(0) ;
                    rightDrive.setPower(0) ;
                    leftDrive.setPower(0) ;
                    rightDrive2.setPower(0) ;
                    sleep(100);
                   /* leftDrive2.setPower(1) ;
                    rightDrive.setPower(-1) ;
                    leftDrive.setPower(-1) ;
                    rightDrive2.setPower(1) ;*/
 
                    sleep(30000);
 
                    leftDrive2.setPower(-0.5) ;
                    rightDrive.setPower(0.5) ;
                    leftDrive.setPower(0.5) ;
                    rightDrive2.setPower(-0.5) ;
 
                    sleep(400);
 
                    leftDrive2.setPower(0) ;
                    rightDrive.setPower(0) ;
                    leftDrive.setPower(0) ;
                    rightDrive2.setPower(0) ;
 
 
 
                    sleep(1000);
 
                    sleep(30000);
 
 
 
 
                   } 
                      int i = 0;
                      for (Recognition recognition : updatedRecognitions) {
                        telemetry.addData(String.format("label (%d)", i), recognition.getLabel());
                        telemetry.addData(String.format("  left,top (%d)", i), "%.03f , %.03f",
                                recognition.getLeft(), recognition.getTop());
                        telemetry.addData(String.format("  right,bottom (%d)", i), "%.03f , %.03f",
                                recognition.getRight(), recognition.getBottom()); 
                                          if (recognition.getLabel().equals("Single")) {
                        telemetry.addData("TargetZone", "B");
                        telemetry.update();
 
              // WobbleArm.setPower(-1);
                    //ringclose.setPosition(1);
 
                    sleep(1000);
 
                    //armvert.setPosition(-1);
                    ringclose.setPosition(1);
                    leftDrive2.setPower(-0.5) ;
                    rightDrive.setPower(0.5) ;
                    leftDrive.setPower(0.5) ;
                    rightDrive2.setPower(-0.5) ;
 
                    sleep(3300);
 
                    leftDrive2.setPower(0) ;
                    rightDrive.setPower(0) ;
                    leftDrive.setPower(0) ;
                    rightDrive2.setPower(0) ;
 
                    sleep(500);
 
                   WobbleArm.setPower(0.7);
 
                    sleep(700);
 
                    ringclose.setPosition(-1);
 
                    sleep(1000);
 
                    WobbleArm.setPower(-0.8);
                    sleep(200);
                    ringclose.setPosition(-1);
 
                    sleep(1000);
                    ringclose.setPosition(-1);
                    leftDrive2.setPower(0.5) ;
                    rightDrive.setPower(-0.5) ;
                    leftDrive.setPower(-0.5) ;
                    rightDrive2.setPower(0.5) ;
                    sleep(3650);
 
                    leftDrive2.setPower(0.5) ;
                    rightDrive.setPower(0.5) ;
                    leftDrive.setPower(-0.5) ;
                    rightDrive2.setPower(-0.5) ;
                    sleep(1750);
                    leftDrive2.setPower(0) ;
                    rightDrive.setPower(0) ;
                    leftDrive.setPower(0) ;
                    rightDrive2.setPower(0) ;
                    sleep(100);
                 /*   WobbleArm.setPower(0.4);
                    sleep(900);
                    ringclose.setPosition(1);
                    sleep(700);
                    WobbleArm.setPower(-0.8);
                    *///sleep(100);
                    leftDrive2.setPower(-0.8) ;
                    rightDrive.setPower(0.8) ;
                    leftDrive.setPower(0.8) ;
                    rightDrive2.setPower(-0.8) ;
                    sleep(1900);
                 /*   leftDrive2.setPower(1) ;
                    rightDrive.setPower(1) ;
                    leftDrive.setPower(-1) ;
                    rightDrive2.setPower(-1) ;
                    */
                    sleep(700);
                    leftDrive2.setPower(0) ;
                    rightDrive.setPower(0) ;
                    leftDrive.setPower(0) ;
                    rightDrive2.setPower(0) ;
                    sleep(100);
                   /* leftDrive2.setPower(1) ;
                    rightDrive.setPower(-1) ;
                    leftDrive.setPower(-1) ;
                    rightDrive2.setPower(1) ;*/
 
                    sleep(30000);
 
 
                        } else if  (recognition.getLabel().equals("Quad")) {
                    telemetry.addData("TargetZone", "C");
                    telemetry.update();
 
             // WobbleArm.setPower(-1);
                    //ringclose.setPosition(1);
 
                    sleep(1000);
 
                    //armvert.setPosition(-1);
                    ringclose.setPosition(1);
                    leftDrive2.setPower(-0.5) ;
                    rightDrive.setPower(0.5) ;
                    leftDrive.setPower(0.5) ;
                    rightDrive2.setPower(-0.5) ;
 
                    sleep(5400);
 
                    leftDrive2.setPower(0) ;
                    rightDrive.setPower(0) ;
                    leftDrive.setPower(0) ;
                    rightDrive2.setPower(0) ;
 
                    sleep(500);
 
                   WobbleArm.setPower(0.7);
 
                    sleep(700);
 
                    ringclose.setPosition(-1);
 
                    sleep(1000);
 
                    WobbleArm.setPower(-0.8);
                    sleep(200);
                    ringclose.setPosition(-1);
 
                    sleep(1000);
                    ringclose.setPosition(-1);
                    leftDrive2.setPower(0.5) ;
                    rightDrive.setPower(-0.5) ;
                    leftDrive.setPower(-0.5) ;
                    rightDrive2.setPower(0.5) ;
                    sleep(6400);
 
                    leftDrive2.setPower(0.5) ;
                    rightDrive.setPower(0.5) ;
                    leftDrive.setPower(-0.5) ;
                    rightDrive2.setPower(-0.5) ;
                    sleep(1690);
                    leftDrive2.setPower(0) ;
                    rightDrive.setPower(0) ;
                    leftDrive.setPower(0) ;
                    rightDrive2.setPower(0) ;
                    sleep(100);
                 /*   WobbleArm.setPower(0.4);
                    sleep(900);
                    ringclose.setPosition(1);
                    sleep(700);
                    WobbleArm.setPower(-0.8);
                    *///sleep(100);
                    leftDrive2.setPower(-0.8) ;
                    rightDrive.setPower(0.8) ;
                    leftDrive.setPower(0.8) ;
                    rightDrive2.setPower(-0.8) ;
                    sleep(3300);
                 /*   leftDrive2.setPower(1) ;
                    rightDrive.setPower(1) ;
                    leftDrive.setPower(-1) ;
                    rightDrive2.setPower(-1) ;
                    */
                    sleep(700);
                    leftDrive2.setPower(0.8) ;
                    rightDrive.setPower(-0.8) ;
                    leftDrive.setPower(-0.8) ;
                    rightDrive2.setPower(0.8);
                    sleep(600);
                    leftDrive2.setPower(0) ;
                    rightDrive.setPower(0) ;
                    leftDrive.setPower(0) ;
                    rightDrive2.setPower(0) ;
                    sleep(100);
                   /* leftDrive2.setPower(1) ;
                    rightDrive.setPower(-1) ;
                    leftDrive.setPower(-1) ;
                    rightDrive2.setPower(1) ;*/
 
                    sleep(30000);
 
                      }
                      }
                      telemetry.update();
 
                    }
                }
            }
            }
        }
 
 
        if (tfod != null) {
            tfod.shutdown();
        }
    }
 
  
    private void initVuforia() {
        /*
         * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
         */
        VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
 
        parameters.vuforiaLicenseKey = VUFORIA_KEY;
        parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");
 
        //  Instantiate the Vuforia engine
        vuforia = ClassFactory.getInstance().createVuforia(parameters);
 
        // Loading trackables is not necessary for the TensorFlow Object Detection engine.
    }
 
    
    // Initialize the TensorFlow Object Detection engine.
   
    private void initTfod() {
        int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
            "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
        TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
       tfodParameters.minResultConfidence = 0.5f;
       tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
       tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABEL_FIRST_ELEMENT, LABEL_SECOND_ELEMENT);
    }
}