TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DRSS_NewBlueAutonomous.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");
        /*The following series of try/catch statements attempt to assign each declared hardware
         *devices to a physical hardware device on the robot
         */
        try {
            leftMotor = hardwareMap.dcMotor.get("leftMotor");
            leftMotor.setDirection(DcMotor.Direction.FORWARD);
        } catch (Exception eTopMotor) {
            leftMotor = null;
        }
        try {
            rightMotor = hardwareMap.dcMotor.get("rightMotor");
            rightMotor.setDirection(DcMotor.Direction.REVERSE);
        } catch (Exception eBottomMotor) {
            rightMotor = null;
        }
        if ((leftMotor != null) && rightMotor != null) {
            leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
            idle();
            leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
            rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
        }
        try{
            particleAccelerator = hardwareMap.dcMotor.get("particleAccelerator");
        } 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;
            telemetry.addData("Warning: ", "ShooterMotorController could not be found");
        }
        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 {
            gyro = (ModernRoboticsI2cGyro) hardwareMap.gyroSensor.get("gyro");
        } catch (Exception eGyroSensor) {
            gyro = null;
        }
        try{
            pushRod = hardwareMap.crservo.get("pushRod");
        } catch ( Exception ePushRod){
            pushRod = null;
        }
        if(pushRod != null) {
            pushRod.setPower(-1.0);
        }
        try{
            cannonSeal = hardwareMap.servo.get("cannonSeal");
        } catch(Exception eCannonSeal){
            telemetry.addData("Exception ", "cannonSeal not found");
            cannonSeal = new DummyServo();
        }
        try{
            opticalDistanceSensor = hardwareMap.opticalDistanceSensor.get("opticalSensor");
        } catch(Exception eOpticalDistanceSensor){
            telemetry.addData("Excpetion ", "opticalSensor not found");
            opticalDistanceSensor = new DummyOpticalDistanceSensor();
        }
        if (gyro != null) {
            gyro.calibrate();
            while (gyro.isCalibrating()) {
                Thread.sleep(50);
                idle();
            }
            telemetry.addData(">", "Gyro Calibrated.  Press Start.");
        } else {
            telemetry.addData("ERROR", "GYRO NOT INTIALIZED");
        }
        telemetry.update();
        waitForStart();
        while (!isFinished && !isStopRequested()) {
            /* Do the following until the robot finishes its autonomous route or a stop
             * has been requested by the robot controller
             */
            telemetry.update();
            heading = gyro.getHeading();
            heading--;
            /* Get the heading (essentialy degrees rotation on the z, or veritical axis) from the
             * gyro. The gyro increments the heading by one degree each time it is read, so the
             * variable storing the heading decrements by one degree each time it is assigned.
             */
            if(resetRequested){
                leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
                rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
                idle();
                leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
                rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
                gyro.resetZAxisIntegrator();
                pushRod.setPower(-1.0);
                runTime.reset();
                resetRequested = false;
                /* If a reset is requested, reset all motor encoders, the gyro heading, and the
                 * internal runtime and end the reset request
                 */
            }
            /* The following section of the program is broken down into several "steps" that
             * represent either a turn, forward movement, or a shot from the particle accelerator by
             * the robot. A variable called target is assigned a value either representing linear
             * distance or angular rotation at the start of each step. The robot is then assigned an
             * action to perform based on the value variable "target" is assigned to. Occasionally,
             *  telemetry info is also passed back to the robot controller.
             */
            if (currentStep == 1) {
                target = 1600;
                driveStraight(target);
                telemetry.addData("Motor1", leftMotor.getCurrentPosition());
                telemetry.addData("Motor2", rightMotor.getCurrentPosition());
                telemetry.addData("current position", currentStep);
            }
            else if (currentStep == 2) {
                shoot();
                telemetry.addData("particleAccelerator", particleAccelerator.getCurrentPosition());
            } else if (currentStep == 2) {
                target = 3300;
                telemetry.addData("lMotor", leftMotor.getCurrentPosition());
                telemetry.addData("rMotor", rightMotor.getCurrentPosition());
                telemetry.addData("lTarget", lTarget);
                telemetry.addData("rTarget", rTarget);
                driveStraight(target);
            } else if (currentStep == 3) {
                gyroTarget = -45;
                turn(gyroTarget);
                telemetry.addData("lMotor", leftMotor.getCurrentPosition());
                telemetry.addData("rMotor", rightMotor.getCurrentPosition());
                telemetry.addData("lTarget", lTarget);
                telemetry.addData("rTarget", rTarget);
            } else if(currentStep == 4) {
                target = 4300;
                driveStraight(target);
            } else if(currentStep == 5) {
                gyroTarget = -125;
                turn(gyroTarget);
            } else if(currentStep == 6){
                target = -3000;
                driveStraight(target);
            } else if(currentStep == 7) {
                gyroTarget = 7;
                turn(gyroTarget);
            } else if(currentStep == 8){
                detectColor();
            } else if(currentStep == 9){
                target = 3800;
                driveStraight(target);
            } else if(currentStep == 10){
                detectColor();
            } else if(currentStep == 11){
                gyroTarget = 7;
            } else if(currentStep == 12){
                target = 2000;
                driveStraight(target);
            } else{
                leftMotor.setPower(0.0);
                rightMotor.setPower(0.0);
            }
        }
    }
public void runOpMode ( ) throws InterruptedException { telemetry . addData ( "Status" , "Initializing" ) ; /*The following series of try/catch statements attempt to assign each declared hardware
         *devices to a physical hardware device on the robot
         */ try { leftMotor = hardwareMap . dcMotor . get ( "leftMotor" ) ; leftMotor . setDirection ( DcMotor . Direction . FORWARD ) ; } catch ( Exception eTopMotor ) { leftMotor = null ; } try { rightMotor = hardwareMap . dcMotor . get ( "rightMotor" ) ; rightMotor . setDirection ( DcMotor . Direction . REVERSE ) ; } catch ( Exception eBottomMotor ) { rightMotor = null ; } if ( ( leftMotor != null ) && rightMotor != null ) { leftMotor . setMode ( DcMotor . RunMode . STOP_AND_RESET_ENCODER ) ; rightMotor . setMode ( DcMotor . RunMode . STOP_AND_RESET_ENCODER ) ; idle ( ) ; leftMotor . setMode ( DcMotor . RunMode . RUN_USING_ENCODER ) ; rightMotor . setMode ( DcMotor . RunMode . RUN_USING_ENCODER ) ; } try { particleAccelerator = hardwareMap . dcMotor . get ( "particleAccelerator" ) ; } 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 ; telemetry . addData ( "Warning: " , "ShooterMotorController could not be found" ) ; } 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 { gyro = ( ModernRoboticsI2cGyro ) hardwareMap . gyroSensor . get ( "gyro" ) ; } catch ( Exception eGyroSensor ) { gyro = null ; } try { pushRod = hardwareMap . crservo . get ( "pushRod" ) ; } catch ( Exception ePushRod ) { pushRod = null ; } if ( pushRod != null ) { pushRod . setPower ( - 1.0 ) ; } try { cannonSeal = hardwareMap . servo . get ( "cannonSeal" ) ; } catch ( Exception eCannonSeal ) { telemetry . addData ( "Exception " , "cannonSeal not found" ) ; cannonSeal = new DummyServo ( ) ; } try { opticalDistanceSensor = hardwareMap . opticalDistanceSensor . get ( "opticalSensor" ) ; } catch ( Exception eOpticalDistanceSensor ) { telemetry . addData ( "Excpetion " , "opticalSensor not found" ) ; opticalDistanceSensor = new DummyOpticalDistanceSensor ( ) ; } if ( gyro != null ) { gyro . calibrate ( ) ; while ( gyro . isCalibrating ( ) ) { Thread . sleep ( 50 ) ; idle ( ) ; } telemetry . addData ( ">" , "Gyro Calibrated.  Press Start." ) ; } else { telemetry . addData ( "ERROR" , "GYRO NOT INTIALIZED" ) ; } telemetry . update ( ) ; waitForStart ( ) ; while ( ! isFinished && ! isStopRequested ( ) ) { /* Do the following until the robot finishes its autonomous route or a stop
             * has been requested by the robot controller
             */ telemetry . update ( ) ; heading = gyro . getHeading ( ) ; heading -- ; /* Get the heading (essentialy degrees rotation on the z, or veritical axis) from the
             * gyro. The gyro increments the heading by one degree each time it is read, so the
             * variable storing the heading decrements by one degree each time it is assigned.
             */ if ( resetRequested ) { leftMotor . setMode ( DcMotor . RunMode . STOP_AND_RESET_ENCODER ) ; rightMotor . setMode ( DcMotor . RunMode . STOP_AND_RESET_ENCODER ) ; idle ( ) ; leftMotor . setMode ( DcMotor . RunMode . RUN_USING_ENCODER ) ; rightMotor . setMode ( DcMotor . RunMode . RUN_USING_ENCODER ) ; gyro . resetZAxisIntegrator ( ) ; pushRod . setPower ( - 1.0 ) ; runTime . reset ( ) ; resetRequested = false ; /* If a reset is requested, reset all motor encoders, the gyro heading, and the
                 * internal runtime and end the reset request
                 */ } /* The following section of the program is broken down into several "steps" that
             * represent either a turn, forward movement, or a shot from the particle accelerator by
             * the robot. A variable called target is assigned a value either representing linear
             * distance or angular rotation at the start of each step. The robot is then assigned an
             * action to perform based on the value variable "target" is assigned to. Occasionally,
             *  telemetry info is also passed back to the robot controller.
             */ if ( currentStep == 1 ) { target = 1600 ; driveStraight ( target ) ; telemetry . addData ( "Motor1" , leftMotor . getCurrentPosition ( ) ) ; telemetry . addData ( "Motor2" , rightMotor . getCurrentPosition ( ) ) ; telemetry . addData ( "current position" , currentStep ) ; } else if ( currentStep == 2 ) { shoot ( ) ; telemetry . addData ( "particleAccelerator" , particleAccelerator . getCurrentPosition ( ) ) ; } else if ( currentStep == 2 ) { target = 3300 ; telemetry . addData ( "lMotor" , leftMotor . getCurrentPosition ( ) ) ; telemetry . addData ( "rMotor" , rightMotor . getCurrentPosition ( ) ) ; telemetry . addData ( "lTarget" , lTarget ) ; telemetry . addData ( "rTarget" , rTarget ) ; driveStraight ( target ) ; } else if ( currentStep == 3 ) { gyroTarget = - 45 ; turn ( gyroTarget ) ; telemetry . addData ( "lMotor" , leftMotor . getCurrentPosition ( ) ) ; telemetry . addData ( "rMotor" , rightMotor . getCurrentPosition ( ) ) ; telemetry . addData ( "lTarget" , lTarget ) ; telemetry . addData ( "rTarget" , rTarget ) ; } else if ( currentStep == 4 ) { target = 4300 ; driveStraight ( target ) ; } else if ( currentStep == 5 ) { gyroTarget = - 125 ; turn ( gyroTarget ) ; } else if ( currentStep == 6 ) { target = - 3000 ; driveStraight ( target ) ; } else if ( currentStep == 7 ) { gyroTarget = 7 ; turn ( gyroTarget ) ; } else if ( currentStep == 8 ) { detectColor ( ) ; } else if ( currentStep == 9 ) { target = 3800 ; driveStraight ( target ) ; } else if ( currentStep == 10 ) { detectColor ( ) ; } else if ( currentStep == 11 ) { gyroTarget = 7 ; } else if ( currentStep == 12 ) { target = 2000 ; driveStraight ( target ) ; } else { leftMotor . setPower ( 0.0 ) ; rightMotor . setPower ( 0.0 ) ; } } }
Only used for future proportional control logic when driving forward.
Only used for future proportional control logic when driving forward .
//      leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); //      rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); //If the push rod is assigned, suck it into the robot //If the gyro is assigned, calibrate and do not do anything else until it finishes //If the gyro is not assigned, declare so. //Send telemetry data to robot controller //Do not run the following code until that start button is pressed. //Send telemetry data to the robot controller //lTarget = -1000; //rTarget = 1000; //turn(lTarget, rTarget); //gyroTarget = -90;