FTC 14169 Autonomous with Camera and IMU

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.Range;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.hardware.bosch.BNO055IMU;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaCurrentGame;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import org.firstinspires.ftc.robotcore.external.tfod.TfodCurrentGame;
import org.firstinspires.ftc.robotcore.external.navigation.Position;
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import java.lang.Math;
import java.util.Set;
import java.util.List;
import java.lang.annotation.Target;

@Autonomous(name="NewAutoCamera", group="2021 Ultimate Goal")
public class NewAutoCamera extends LinearOpMode
{
    // Set Timer 
    private ElapsedTime  runtime = new ElapsedTime();
    
    
    // @TODO: Intialize a variable to store the ring count. You might update this later in the code.
    // camera recognition and tensor flow
    private VuforiaCurrentGame vuforiaUltimateGoal;
    private TfodCurrentGame tfodUltimateGoal;
    
    // number of rings camera finds
    Recognition recognition;
    
    //declare variable
    private int ringCount = 0;
  
    // declare motors
    private DcMotor right;
    private DcMotor left;
    private DcMotor center;
    private CRServo intake;
    private CRServo launcher;
    private Servo claw;
    private Servo arm;
    private Servo flicker;
    private BNO055IMU imu;
    
    // encoder clicks are originally 1680
    // multiply that by drive train gear ratio (two thirds)   
    private static final int ENCODER_CLICKS = 1680;
    private static final double WHEEL_DIAM = 10.0;
    private static final double WHEEL_CIRC = WHEEL_DIAM * Math.PI;
    private static final double DRIVE_GEAR_RATIO = 2/3;
    private static final double CLICKS_PER_CM = ENCODER_CLICKS / WHEEL_CIRC;
    
    // Declare IMU 
    BNO055IMU.Parameters IMU_Parameters;
    ElapsedTime ElapsedTime2;
    double Left_Power;
    double Right_Power;
    double Original_Power;
    float Yaw_Angle = 0;   
    
    
    
    @Override
    public void runOpMode()
    {
        // Telemetry to show OpMode is initialized
        telemetry.addData("Status", "Initialized");
        telemetry.update();
        
        // list rings
        List<Recognition> recognitions;
        double index;
    
        vuforiaUltimateGoal = new VuforiaCurrentGame();
        tfodUltimateGoal = new TfodCurrentGame();

        // Initialize the hardware variables. 
        left  = hardwareMap.get(DcMotor.class, "left");
        right = hardwareMap.get(DcMotor.class, "right");
        center = hardwareMap.get(DcMotor.class, "center");
        intake = hardwareMap.get(CRServo.class, "intake");
        launcher = hardwareMap.get(CRServo.class, "launcher");
        claw = hardwareMap.get(Servo.class, "claw");
        arm = hardwareMap.get(Servo.class, "arm");
        flicker = hardwareMap.get(Servo.class, "flicker");
         imu = hardwareMap.get(BNO055IMU.class, "imu");
        
        // Initialize motor direction
        right.setDirection(DcMotor.Direction.FORWARD);
        left.setDirection(DcMotor.Direction.REVERSE);
        center.setDirection(DcMotor.Direction.REVERSE);
        
    
        // Reset motor encoders
        right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        left.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        center.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
        
        // Initialize motor encoder modes
        right.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        left.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        center.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        
        
        launcher.setDirection(DcMotorSimple.Direction.FORWARD);
        intake.setDirection(DcMotorSimple.Direction.FORWARD);
        claw.setDirection(Servo.Direction.FORWARD);
        arm.setDirection(Servo.Direction.FORWARD);
        flicker.setDirection(Servo.Direction.FORWARD);
        
        // Initialize IMU 
        IMU_Parameters = new BNO055IMU.Parameters();
        IMU_Parameters.mode = BNO055IMU.SensorMode.IMU;
        imu.initialize(IMU_Parameters);   
        
        telemetry.addData("Status", "IMU initialized, calibration started.");
        telemetry.update();
        
         // Calibrate IMU       
        while (!IMU_Calibrated()) {
          telemetry.addData("If calibration ", "doesn't complete after 3 seconds, move through 90 degree pitch, roll and yaw motions until calibration complete ");
          telemetry.update();
          // Wait one second before checking calibration status again.
          sleep(1000);
        }
           
        telemetry.addData("Status", "Calibration Complete");
        telemetry.update();
        
        
        // Initialize Vuforia.
        // We need Vuforia to provide TFOD with camera images.
        vuforiaUltimateGoal.initialize(
            "AfacJkb/////AAABmYDDu+vj6knuuMS/g+9F+bwOQBEMB9uDwPrDbCTt+2vv8caD/rZ47CeNGlqKRzd8sEWyv/N5c051wbC9mOBXx0qSafGm7pPqaNbk5B7fF/mXuvBPcsfbxtM1hHrWtS+xxZKafEyC63AAIBWp6nPsbhV/LwN8F65750TQg9WZA4yuFGpAxuNxSW61KdKs6kU12MzWz0chRLtZLWbFJiwB4jy5KteBeL9+PzTgw4FWBfVnSHxnC2sVCyk/O7ZxPTD+v0bKQLusNNW3CV1U+PXWXjWr3Najo2v660Ui4O999r12jmtwSARQWMmzh9EaaftO2R79Bv4y8RKlhV4G2k/E25szK4PI/B/YzrGtxepL3zNQ", // vuforiaLicenseKey
            VuforiaLocalizer.CameraDirection.BACK, // cameraDirection
            false, // useExtendedTracking
            false, // enableCameraMonitoring
            VuforiaLocalizer.Parameters.CameraMonitorFeedback.AXES, // cameraMonitorFeedback
            0, // dx
            0, // dy
            0, // dz
            0, // xAngle
            -90, // yAngle
            0, // zAngle
            true); // useCompetitionFieldTargetLocations
        
        // Set min confidence threshold 
        tfodUltimateGoal.initialize(vuforiaUltimateGoal, 0.1F, true, true);
        
        // Initialize TFOD before waitForStart.
        // Init TFOD here so the object detection labels are visible
        // in the Camera Stream preview window on the Driver Station.
        tfodUltimateGoal.activate();
        
        // Enable following block to zoom in on target.
        tfodUltimateGoal.setZoom(1, 16 / 9);
        telemetry.addData(">", "Press Play to start");
        telemetry.update();
        

        telemetry.addData("status","init" );
        telemetry.update();


        // OpMode needs waitForStart(); and while (opModeIsActive()) {...}
        waitForStart();

        telemetry.addData("status","op mode started" );
        telemetry.update();
        
        // list number of rings found
        recognitions = tfodUltimateGoal.getRecognitions();
        
        // If list is empty, inform the user. Otherwise, go
        // through list and display info for each recognition.
        if (recognitions.size() == 0) {
            
          telemetry.addData("TFOD", "No items detected.");
          telemetry.addData("Target Zone", "A");
        
        } else {
          index = 0;
          // Iterate through list and call a function to
          // display info for each recognized object.
          for (Recognition recognition_item : recognitions) {
            recognition = recognition_item;
            
            if (recognition.getLabel().equals("Single")){
              ringCount=1;
            }else{
              ringCount=4;
            }
            // Increment index.
            index = index + 1;
          }
        }
        
        telemetry.addData("status","ringCount,  %7d", ringCount );
        telemetry.update();
        sleep(1000);
        
        // Deactivate TFOD.
        tfodUltimateGoal.deactivate();
        vuforiaUltimateGoal.close();
        tfodUltimateGoal.close();
        
        
        
        //Automonous Drive Code Starts Here
       
        if (ringCount == 1) {
            
            
            flicker.setPosition(0);
            sleep(1000);
            
            launcher.setPower(-0.8);
            sleep(5000);
            
            flicker.setPosition(0.5);
            sleep(1000);
            
            flicker.setPosition(0);
            sleep(1000);
            
            // Drive Forward
            driveBot(91.4,91.4,0.4,5.0);
            telemetry.addData("status","first run to position called" );
            telemetry.addData("status", left.getMode() );
            telemetry.addData("status","left motor,  %7d", left.getCurrentPosition() );
            telemetry.addData("status","right motor,  %7d", right.getCurrentPosition() );
            telemetry.update();
            right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            left.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            sleep(1000);
            
            turn(true, 90);
            sleep(1000);
        
        
            // Strafe Left
            center.setTargetPosition(1680);
            center.setPower(0.45);
            sleep(1000);
            center.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            center.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            center.setPower(0);
            sleep(1000);

            // Launch
            //launcher.setPower(0.78);
            sleep(1000);
        

            //Arm and claw
        
            
        }
        // End RingCount 1
        
        if (ringCount == 0) {
            
            flicker.setPosition(0);
            sleep(1000);
            
            launcher.setPower(-0.8);
            sleep(5000);
            
            flicker.setPosition(0.5);
            sleep(1000);
            
            flicker.setPosition(0);
            sleep(1000);
            
            driveBot(91.4,91.4,0.4,5.0);
            telemetry.addData("status","first run to position called" );
            telemetry.addData("status", left.getMode() );
            telemetry.addData("status","left motor,  %7d", left.getCurrentPosition() );
            telemetry.addData("status","right motor,  %7d", right.getCurrentPosition() );
            telemetry.update();
            right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            left.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            sleep(1000);
            
            center.setTargetPosition(1680);
            center.setPower(0.45);
            sleep(1000);
            center.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            center.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            center.setPower(0);
            
            //launcher.setPower(0.78);
            sleep(1000);

/*            
            driveBot(-19.0,19.0,00.3,5.0);
            right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            left.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            sleep(1000);
*/            
            turn(true, 90);
            sleep(1000);
            
            // Arm down, claw
        
            
        }
        // End RingCount 0
        
        if (ringCount == 4) {
            
            flicker.setPosition(0);
            sleep(1000);
            
            launcher.setPower(-0.8);
            sleep(5000);
            
            flicker.setPosition(0.5);
            sleep(1000);
            
            flicker.setPosition(0);
            sleep(1000);
            
            driveBot(91.4,91.4,0.4,5.0);
            telemetry.addData("status","first run to position called" );
            telemetry.addData("status", left.getMode() );
            telemetry.addData("status","left motor,  %7d", left.getCurrentPosition() );
            telemetry.addData("status","right motor,  %7d", right.getCurrentPosition() );
            telemetry.update();
            right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            left.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            sleep(1000);
            
            center.setTargetPosition(1680);
            center.setPower(0.45);
            sleep(1000);
            center.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            center.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            center.setPower(0);
            
            //launcher.setPower(0.78);
            sleep(1000);
            
            driveBot(60.9,60.9,00.3,5.0);
            right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            left.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            sleep(1000);


/*            
            driveBot(-19.0,19.0,00.3,5.0);
            right.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            left.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            sleep(1000);
*/

            turn(true, 90);
            sleep(1000);
            
            // Arm down, claw
            
            center.setTargetPosition(3360);
            center.setPower(0.45);
            sleep(1000);
            center.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            center.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            center.setPower(0);
        
            
        }
        //End RingCount 4
    }
    // END runOpMode()
    
    
    
    
    /**********************************************
     * BEGIN METHODS driveDistance, driveBot
     *********************************************/
    
    /**
    * Drive Distance Method
    * Calculate distance from CM to encoder counts
    */  
    public static double driveDistance(double distance)
    {
        double drive  = (ENCODER_CLICKS/ WHEEL_CIRC);
        int outputClicks= (int)Math.floor(drive * distance);
        return outputClicks;
    }
    // END driveDistance() method


    /**
    * Drive Method
    * Left & Right travel distance in CM +/-, power to both wheels, timeout in seconds
    */
    public void driveBot(double distanceInCMleft, double distanceInCMright, double power, double timeoutS) 
    {
        telemetry.addData("status","encoder reset");
        telemetry.update();
        
        int rightTarget;
        int leftTarget;

        if(opModeIsActive()) 
        {
            telemetry.addData("status","getEncoderClicks");
            telemetry.update();
            
            rightTarget = (int) driveDistance(distanceInCMright);
            leftTarget = (int) driveDistance(distanceInCMleft);

            right.setTargetPosition(rightTarget);
            left.setTargetPosition(leftTarget);

            right.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            left.setMode(DcMotor.RunMode.RUN_TO_POSITION);
            
            runtime.reset();

            right.setPower(power);
            left.setPower(power);
        
            while (opModeIsActive() &&
                (runtime.seconds() < timeoutS) &&
                (left.isBusy() && right.isBusy()))
                {
                    telemetry.addData("Path1", "leftTarget, rightTarget" );
                    telemetry.update();
                }
            left.setPower(0);
            right.setPower(0);
            left.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            right.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        }   
    }
    // END driveBot method
    
    
    /**
    * Turn left with IMU
    * Left travel distance in degrees  +/-, power to both wheels, timeout in seconds
    */
    public void turnLeft()
    {
        //MOVE RIGHT
        left.setPower(-0.2);
        right.setPower(0.2);
        
        // Continue until robot yaws right by 90 degrees or stop is pressed on Driver Station.
        sleep(1000);
        //turns to the right; 90, 180, negative, -90, 0
        while ( !(Yaw_Angle <= -85 || isStopRequested()) ) {
            // Update Yaw-Angle variable with current yaw.
            Yaw_Angle = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
            // Report yaw orientation to Driver Station.
            telemetry.addData("Yaw value", Yaw_Angle);
            telemetry.update();
        }
        // We're done. Turn off motors
        left.setPower(0);
        right.setPower(0);
        // Pause so final telemetry is displayed.
        sleep(1000);
    }
    // END turnLeft
    

    /**
    * Turn right with IMU
    * Left & Right travel distance in degrees +/-, power to both wheels, timeout in seconds
    */
    public void turnRight()
    {
        //MOVE RIGHT
        left.setPower(0.2);
        right.setPower(-0.2);
        // Continue until robot yaws right by 90 degrees or stop is pressed on Driver Station.
        sleep(1000);
        //turns to the right; 90, 180, negative, -90, 0
        while ( !(Yaw_Angle >= 85 || isStopRequested()) ) {
            // Update Yaw-Angle variable with current yaw.
            Yaw_Angle = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
            // Report yaw orientation to Driver Station.
            telemetry.addData("Yaw value", Yaw_Angle);
            telemetry.update();
        }
        // We're done. Turn off motors
        left.setPower(0);
        right.setPower(0);
        // Pause so final telemetry is displayed.
        sleep(1000);
    }
    // END turnRight method
    
    
    /**
    * Turn  with IMU
    * true for right, degrees are for the direction you're going in
    * Left & Right travel distance in degrees +/-, power to both wheels, timeout in seconds
    */
    public void turn(boolean dir, int degrees)
    {
            if(!dir)
            {
                //MOVE LEFT
                left.setPower(-0.2);
                right.setPower(0.2);
                degrees = (int)(degrees * 0.94444444444);
                
                //turns to the right; 90, 180, negative, -90, 0
                sleep(1000);
                while ( !(Yaw_Angle <= (-1*degrees) || isStopRequested()) ) {
                    // Update Yaw-Angle variable with current yaw.
                    Yaw_Angle = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
                    // Report yaw orientation to Driver Station.
                    telemetry.addData("Yaw value", Yaw_Angle);
                    telemetry.update();
                }
            }
            else
            {
            //MOVE RIGHT
            left.setPower(0.2);
            right.setPower(-0.2);
            degrees = (int)(degrees * 0.94444444444);
            
            sleep(1000);
            while ( !(Yaw_Angle >= degrees || isStopRequested()) ) {
                // Update Yaw-Angle variable with current yaw.
                Yaw_Angle = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES).thirdAngle;
                // Report yaw orientation to Driver Station.
                telemetry.addData("Yaw value", Yaw_Angle);
                telemetry.update();
            }
        }
        // We're done. Turn off motors
        left.setPower(0);
        right.setPower(0);
        // Pause so final telemetry is displayed.
        sleep(1000);
    }
    // END turn method
    
    
    /**
    * IMU Calibration Method
    * Checks IMU calibration and returns telementry
    * If IMU is NOT calibrated, run the calibration opMode 
    */ 
        private boolean IMU_Calibrated() {
        telemetry.addData("IMU Calibration Status", imu.getCalibrationStatus());
        telemetry.addData("Gyro Calibrated", imu.isGyroCalibrated() ? "True" : "False");
        telemetry.addData("System Status", imu.getSystemStatus().toString());
        return imu.isGyroCalibrated();
    }
}