BlueAuto

public class AUTOMATION extends LinearOpMode {

      private DcMotor motor4;
      private DcMotor motor3;
      private DcMotor motor1;
      private DcMotor motor2;
      private DcMotor lift;
      private CRServo pivot;
      private Servo grabber;

    /*
     * Specify the source for the Tensor Flow Model.
     * If the TensorFlowLite object model is included in the Robot Controller App as an "asset",
     * the OpMode must to load it using loadModelFromAsset().  However, if a team generated model
     * has been downloaded to the Robot Controller's SD FLASH memory, it must to be loaded using loadModelFromFile()
     * Here we assume it's an Asset.    Also see method initTfod() below .
     */
    private static final String TFOD_MODEL_ASSET = "PowerPlay.tflite";
    private static final String blueCone = "/sdcard/FIRST/tflitemodels/BlueCone.tflite";
    // private static final String TFOD_MODEL_FILE  = "/sdcard/FIRST/tflitemodels/CustomTeamModel.tflite";


    private static final String[] LABELS = {
            "Bolt",
            "Bulb",
            "Panel"
    };

    private static final String[] coneLabels = {
            "bCone",
    };

    private static final String VUFORIA_KEY =
            "Ad4M7GP/////AAABmeZxPeyoWU0vk45dz6VmWoFZtZ3Beei2zVQN6SQjXKbzyNWwHkloibWVtf4m7s4WMhM6+3JUvKVBmBk0tPHahpeDFQi8gZ+x3/44X4M9xBxSoycd6WNwAst5YNIh8fN11Ml7v3tyxEtX7olacEfty/hSqWQm6GfwZBpgqXiJKJkjYy0K8XOutYdsKxVajNc7I636HDd970RxlvB73DgnsiFEtlX5CG/f4UFI1w99II6RKCj8fgFsaihHm1v2iWddGGOF24fcQF8bBejtwa4Hi9rs4ohh4t7yr6hpNraG6avcvtzMQzXhIbe1aWkaFSViHAMMpUDHoV/yIjFop6G7pQ2D7ysHtKidCRrM5Tq6mLaL";

    /**
     * {@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();

        motor4 = hardwareMap.get(DcMotor.class, "motor 4");
        motor3 = hardwareMap.get(DcMotor.class, "motor 3");
        motor1 = hardwareMap.get(DcMotor.class, "motor 1");
        motor2 = hardwareMap.get(DcMotor.class, "motor 2");
        lift = hardwareMap.get(DcMotor.class, "lift");
        pivot = hardwareMap.get(CRServo.class, "pivot");
        grabber = hardwareMap.get(Servo.class, "grabber");


        /**
         * 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 increase 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 16/9).
            tfod.setZoom(1.0, 16.0/9.0);
        }

        /** Wait for the game to begin */
        telemetry.addData(">", "Press Play to start op mode");
        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 && updatedRecognitions.size() > 0) {
                        telemetry.addData("# Objects Detected", updatedRecognitions.size());
                        telemetry.addData(updatedRecognitions.get(0).getLabel(), " ");
                        if (updatedRecognitions.get(0).getLabel() == "Bolt"){
                            motor1.setPower(1);
                            motor2.setPower(1);
                            motor3.setPower(-1);
                            motor4.setPower(-1);
                            sleep(1150);
                            motor1.setPower(-1);
                            motor2.setPower(1);
                            motor3.setPower(-1);
                            motor4.setPower(1);
                            sleep(1300);
                            motor1.setPower(1);
                            motor2.setPower(1);
                            motor3.setPower(-1);
                            motor4.setPower(-1);
                            sleep(200);
                            motor1.setPower(0);
                            motor2.setPower(0);
                            motor3.setPower(0);
                            motor4.setPower(0);

                        }else if(updatedRecognitions.get(0).getLabel() == "Bulb"){
                            motor1.setPower(1);
                            motor2.setPower(1);
                            motor3.setPower(-1);
                            motor4.setPower(-1);
                            sleep(1150);
                            motor1.setPower(0);
                            motor2.setPower(0);
                            motor3.setPower(0);
                            motor4.setPower(0);
                        }else if(updatedRecognitions.get(0).getLabel() == "Panel"){
                           motor1.setPower(1);
                            motor2.setPower(1);
                            motor3.setPower(-1);
                            motor4.setPower(-1);
                            sleep(1000);
                            motor1.setPower(1);
                            motor2.setPower(-1);
                            motor3.setPower(1);
                            motor4.setPower(-1);
                            sleep(1400);
                            motor1.setPower(1);
                            motor2.setPower(1);
                            motor3.setPower(-1);
                            motor4.setPower(-1);
                            sleep(100);
                            motor1.setPower(0);
                            motor2.setPower(0);
                            motor3.setPower(0);
                            motor4.setPower(0);
                        }
                        // step through the list of recognitions and display image position/size information for each one
                        // Note: "Image number" refers to the randomized image orientation/number
                    }
                }
                telemetry.update();
            }
        }
    }

    /**
     * Initialize the Vuforia localization engine.
     */
    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, "Eye");

        //  Instantiate the Vuforia engine
        vuforia = ClassFactory.getInstance().createVuforia(parameters);
    }

    /**
     * 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.75f;
        tfodParameters.isModelTensorFlow2 = true;
        tfodParameters.inputSize = 300;
        tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);

        // Use loadModelFromAsset() if the TF Model is built in as an asset by Android Studio
        // Use loadModelFromFile() if you have downloaded a custom team model to the Robot Controller's FLASH.
        tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS);
        tfod.loadModelFromFile(blueCone, coneLabels);

        // tfod.loadModelFromFile(TFOD_MODEL_FILE, LABELS);
    }
}