TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DRSS_Dumb_Autonomous.java
//Only used for future proportional control logic when driving forward.
Only used for future proportional control logic when driving forward.
Only used for future proportional control logic when driving forward .
public void runOpMode() throws InterruptedException {
        telemetry.addData("Status", "Initializing");
        try {
            leftMotor = hardwareMap.dcMotor.get("leftMotor");
            leftMotor.setDirection(DcMotor.Direction.FORWARD);
            leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        } catch (Exception eTopMotor) {
            leftMotor = null;
        }
        try {
            rightMotor = hardwareMap.dcMotor.get("rightMotor");
            rightMotor.setDirection(DcMotor.Direction.REVERSE);
            rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        } catch (Exception eBottomMotor) {
            rightMotor = null;
        }
        try {
            colorSensor = (ModernRoboticsI2cColorSensor) hardwareMap.colorSensor.get("colorSensor");
        } catch (Exception eColorSensor) {
            colorSensor = null;
            telemetry.addData("Warning: ", "Color Sensor could not be found");
        }
        try {
            driveMotorController = hardwareMap.dcMotorController.get("DriveMotorController");
        } catch (Exception eDriveController) {
            driveMotorController = null;
        }
        try{
            particleAccelerator = hardwareMap.dcMotor.get("particleAccelerator");
            particleAccelerator.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        } catch (Exception eParticleAccelerator){
            particleAccelerator = null;
        }
        try{
            particleLift = hardwareMap.dcMotor.get("particleLift");
        } catch ( Exception eParticleLift){
            particleLift = null;
        }
        try{
            shooterMotorController = hardwareMap.dcMotorController.get("ShooterMotorController");
        } catch(Exception eShooterController){
            shooterMotorController = null;
        }
/*
        try {
            gyro = (ModernRoboticsI2cGyro) hardwareMap.gyroSensor.get("gyro");
        } catch (Exception eGyroSensor) {
            gyro = new ModernRoboticsI2cGyro(null, 0);
        }
*/
        try{
            pushRod = hardwareMap.crservo.get("pushRod");
        } catch ( Exception ePushRod){
            pushRod = null;
        }
        try{
            servoController = hardwareMap.servoController.get("servoController");
        } catch(Exception eServoController){
            servoController = null;
        }
        pushRod.setPower(-1.0);
/*
        if (gyro != null) {
            while (gyro.isCalibrating()) {
                Thread.sleep(50);
                idle();
            }
        } else {
            telemetry.addData("ERROR", "GYRO NOT INTIALIZED");
        }
*/
        telemetry.update();
        waitForStart();
        target = 2800;
        while (!isFinished && !isStopRequested()) {
            telemetry.update();
            heading--;
            if (currentStep == 1) {
                reset();
                particleLift.setPower(1.0);
                driveStraight(target);
                telemetry.addData("Motor1", leftMotor.getCurrentPosition());
                telemetry.addData("Motor2", rightMotor.getCurrentPosition());
                telemetry.addData("current positon", currentStep);
                telemetry.addData("error", error);
                telemetry.addData("left Power", leftMotor.getPower());
                telemetry.addData("right Power", rightMotor.getPower());
                telemetry.addData("Current Z:", heading);
                telemetry.update();
            } else if(currentStep == 2){
                reset();
                leftMotor.setPower(0.0);
                rightMotor.setPower(0.0);
                shoot();
                telemetry.addData("Runtime", runTime);
           /* } else if(currentStep == 3) {
                target = 7000;
                reset();
                particleLift.setPower(0.0);
                driveStraight(target);
                telemetry.addData("Error", "Finished Early");
                telemetry.addData("lMotor", leftMotor.getCurrentPosition());
                telemetry.addData("rMotor", rightMotor.getCurrentPosition());
                telemetry.addData("lTarget", lTarget);
                telemetry.addData("rTarget", rTarget);
          /*  }else if(currentStep == 4){
                gyroTarget = 90;
                reset();
                turn(gyroTarget);
            } else if (currentStep == 5) {
                target = 7000;
                reset();
                driveStraight(target);
                telemetry.addData("lMotor", leftMotor.getCurrentPosition());
                telemetry.addData("rMotor", rightMotor.getCurrentPosition());
                telemetry.addData("lTarget", lTarget);
                telemetry.addData("rTarget", rTarget);
/*
            } else if (currentStep == 6) {
                gyroTarget = -90;
                reset();
                telemetry.addData("lMotor", leftMotor.getCurrentPosition());
                telemetry.addData("rMotor", rightMotor.getCurrentPosition());
                telemetry.addData("lTarget", lTarget);
                telemetry.addData("rTarget", rTarget);
                turn(gyroTarget);
            } else if (currentStep == 7) {
                reset();
                leftMotor.setPower(0.0);
                rightMotor.setPower(0.0);
                detectColor();
                telemetry.addData("red", red);
                telemetry.addData("blue", blue);
                telemetry.update();
            } else if (currentStep == 8) {
                target = 4000;
                reset();
                driveStraight(target);
            } else if( currentStep == 9){
                reset();
                leftMotor.setPower(0.0);
                rightMotor.setPower(0.0);
                detectColor();
                telemetry.addData("red", red);
                telemetry.addData("blue", blue);
                telemetry.update();
            } else if(currentStep == 10){
                gyroTarget = -114;
                reset();
                turn(gyroTarget);
            } else if(currentStep == 11){
                target = 8000;
                reset();
                driveStraight(target);
  */           } else{
                leftMotor.setPower(0.0);
                rightMotor.setPower(0.0);
                particleLift.setPower(0.0);
            }
        }
    }
public void runOpMode ( ) throws InterruptedException { telemetry . addData ( "Status" , "Initializing" ) ; try { leftMotor = hardwareMap . dcMotor . get ( "leftMotor" ) ; leftMotor . setDirection ( DcMotor . Direction . FORWARD ) ; leftMotor . setMode ( DcMotor . RunMode . RUN_USING_ENCODER ) ; } catch ( Exception eTopMotor ) { leftMotor = null ; } try { rightMotor = hardwareMap . dcMotor . get ( "rightMotor" ) ; rightMotor . setDirection ( DcMotor . Direction . REVERSE ) ; rightMotor . setMode ( DcMotor . RunMode . RUN_USING_ENCODER ) ; } catch ( Exception eBottomMotor ) { rightMotor = null ; } try { colorSensor = ( ModernRoboticsI2cColorSensor ) hardwareMap . colorSensor . get ( "colorSensor" ) ; } catch ( Exception eColorSensor ) { colorSensor = null ; telemetry . addData ( "Warning: " , "Color Sensor could not be found" ) ; } try { driveMotorController = hardwareMap . dcMotorController . get ( "DriveMotorController" ) ; } catch ( Exception eDriveController ) { driveMotorController = null ; } try { particleAccelerator = hardwareMap . dcMotor . get ( "particleAccelerator" ) ; particleAccelerator . setMode ( DcMotor . RunMode . RUN_USING_ENCODER ) ; } catch ( Exception eParticleAccelerator ) { particleAccelerator = null ; } try { particleLift = hardwareMap . dcMotor . get ( "particleLift" ) ; } catch ( Exception eParticleLift ) { particleLift = null ; } try { shooterMotorController = hardwareMap . dcMotorController . get ( "ShooterMotorController" ) ; } catch ( Exception eShooterController ) { shooterMotorController = null ; } /*
        try {
            gyro = (ModernRoboticsI2cGyro) hardwareMap.gyroSensor.get("gyro");
        } catch (Exception eGyroSensor) {
            gyro = new ModernRoboticsI2cGyro(null, 0);
        }
*/ try { pushRod = hardwareMap . crservo . get ( "pushRod" ) ; } catch ( Exception ePushRod ) { pushRod = null ; } try { servoController = hardwareMap . servoController . get ( "servoController" ) ; } catch ( Exception eServoController ) { servoController = null ; } pushRod . setPower ( - 1.0 ) ; /*
        if (gyro != null) {
            while (gyro.isCalibrating()) {
                Thread.sleep(50);
                idle();
            }
        } else {
            telemetry.addData("ERROR", "GYRO NOT INTIALIZED");
        }
*/ telemetry . update ( ) ; waitForStart ( ) ; target = 2800 ; while ( ! isFinished && ! isStopRequested ( ) ) { telemetry . update ( ) ; heading -- ; if ( currentStep == 1 ) { reset ( ) ; particleLift . setPower ( 1.0 ) ; driveStraight ( target ) ; telemetry . addData ( "Motor1" , leftMotor . getCurrentPosition ( ) ) ; telemetry . addData ( "Motor2" , rightMotor . getCurrentPosition ( ) ) ; telemetry . addData ( "current positon" , currentStep ) ; telemetry . addData ( "error" , error ) ; telemetry . addData ( "left Power" , leftMotor . getPower ( ) ) ; telemetry . addData ( "right Power" , rightMotor . getPower ( ) ) ; telemetry . addData ( "Current Z:" , heading ) ; telemetry . update ( ) ; } else if ( currentStep == 2 ) { reset ( ) ; leftMotor . setPower ( 0.0 ) ; rightMotor . setPower ( 0.0 ) ; shoot ( ) ; telemetry . addData ( "Runtime" , runTime ) ; /* } else if(currentStep == 3) {
                target = 7000;
                reset();
                particleLift.setPower(0.0);
                driveStraight(target);
                telemetry.addData("Error", "Finished Early");
                telemetry.addData("lMotor", leftMotor.getCurrentPosition());
                telemetry.addData("rMotor", rightMotor.getCurrentPosition());
                telemetry.addData("lTarget", lTarget);
                telemetry.addData("rTarget", rTarget);

          /*  }else if(currentStep == 4){
                gyroTarget = 90;
                reset();
                turn(gyroTarget);

            } else if (currentStep == 5) {
                target = 7000;
                reset();
                driveStraight(target);
                telemetry.addData("lMotor", leftMotor.getCurrentPosition());
                telemetry.addData("rMotor", rightMotor.getCurrentPosition());
                telemetry.addData("lTarget", lTarget);
                telemetry.addData("rTarget", rTarget);

/*
            } else if (currentStep == 6) {
                gyroTarget = -90;
                reset();
                telemetry.addData("lMotor", leftMotor.getCurrentPosition());
                telemetry.addData("rMotor", rightMotor.getCurrentPosition());
                telemetry.addData("lTarget", lTarget);
                telemetry.addData("rTarget", rTarget);
                turn(gyroTarget);


            } else if (currentStep == 7) {
                reset();
                leftMotor.setPower(0.0);
                rightMotor.setPower(0.0);
                detectColor();
                telemetry.addData("red", red);
                telemetry.addData("blue", blue);
                telemetry.update();

            } else if (currentStep == 8) {
                target = 4000;
                reset();
                driveStraight(target);

            } else if( currentStep == 9){
                reset();
                leftMotor.setPower(0.0);
                rightMotor.setPower(0.0);
                detectColor();
                telemetry.addData("red", red);
                telemetry.addData("blue", blue);
                telemetry.update();

            } else if(currentStep == 10){
                gyroTarget = -114;
                reset();
                turn(gyroTarget);

            } else if(currentStep == 11){
                target = 8000;
                reset();
                driveStraight(target);

  */ } else { leftMotor . setPower ( 0.0 ) ; rightMotor . setPower ( 0.0 ) ; particleLift . setPower ( 0.0 ) ; } } }
Only used for future proportional control logic when driving forward.
Only used for future proportional control logic when driving forward .
//Output that the robot is initializing. //        gyro.calibrate(); //       telemetry.addData(">", "Gyro Calibrated.  Press Start."); //target = 2800; //heading = gyro.getHeading();