TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DRSS_CaseSwitchBlueAutonomous.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 = new DummyDcMotor();
        }
        try {
            rightMotor = hardwareMap.dcMotor.get("rightMotor");
            rightMotor.setDirection(DcMotor.Direction.REVERSE);
        } catch (Exception eBottomMotor) {
            rightMotor = new DummyDcMotor();
        }
        if ((leftMotor != null) && rightMotor != null) {
            leftMotor.setMode(STOP_AND_RESET_ENCODER);
            rightMotor.setMode(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 = new DummyDcMotor();
        }
        try{
            particleLift = hardwareMap.dcMotor.get("particleLift");
        } catch ( Exception eParticleLift){
            particleLift = new DummyDcMotor();
        }
        try{
            shooterMotorController = hardwareMap.dcMotorController.get("ShooterMotorController");
        } catch(Exception eShooterController){
            shooterMotorController = new DummyDcMotorController();
            telemetry.addData("Warning: ", "ShooterMotorController could not be found");
        }
        try {
            colorSensor = hardwareMap.colorSensor.get("colorSensor");
            colorSensor.enableLed(true);
        } catch (Exception eColorSensor) {
            colorSensor = new ModernRoboticsI2cColorSensor(null, 0);
            telemetry.addData("Warning: ", "Color Sensor could not be found");
        }
        try {
            driveMotorController = hardwareMap.dcMotorController.get("DriveMotorController");
        } catch (Exception eDriveController) {
            driveMotorController = new DummyDcMotorController();
        }
        try {
            gyro = (ModernRoboticsI2cGyro) hardwareMap.gyroSensor.get("gyro");
        } catch (Exception eGyroSensor) {
            gyro = null;
        }
        try{
            pushRod = hardwareMap.crservo.get("pushRod");
        } catch ( Exception ePushRod){
            pushRod = new DummyCRServo();
        }
        try{
            clawTrigger = hardwareMap.servo.get("clawTrigger");
        } catch(Exception eClawTrigger){
            telemetry.addData("Exception ", "clawTrigger not found");
            clawTrigger = new DummyServo();
        }
        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("Esception", "opticalSenor not found");
            opticalDistanceSensor = new DummyOpticalDistanceSensor();
        }
        if(pushRod != null) {
            pushRod.setPower(-1.0);
        }
        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();
        cannonSeal.setPosition(0.0);
        switch(currentStep){
            case 0:
                driveStraight(0);
                if (!opModeIsActive());
                    break;
            case 1:
                shoot();
                if (!opModeIsActive());
                    break;
            case 2:
                driveStraight(0);
                if (!opModeIsActive());
                    break;
            case 3:
                encodedgpsturn(0);
                if (!opModeIsActive());
                    break;
            case 4:
                driveStraight(0);
                if (!opModeIsActive());
                break;
        }
        stop();
    }
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 = new DummyDcMotor ( ) ; } try { rightMotor = hardwareMap . dcMotor . get ( "rightMotor" ) ; rightMotor . setDirection ( DcMotor . Direction . REVERSE ) ; } catch ( Exception eBottomMotor ) { rightMotor = new DummyDcMotor ( ) ; } if ( ( leftMotor != null ) && rightMotor != null ) { leftMotor . setMode ( STOP_AND_RESET_ENCODER ) ; rightMotor . setMode ( 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 = new DummyDcMotor ( ) ; } try { particleLift = hardwareMap . dcMotor . get ( "particleLift" ) ; } catch ( Exception eParticleLift ) { particleLift = new DummyDcMotor ( ) ; } try { shooterMotorController = hardwareMap . dcMotorController . get ( "ShooterMotorController" ) ; } catch ( Exception eShooterController ) { shooterMotorController = new DummyDcMotorController ( ) ; telemetry . addData ( "Warning: " , "ShooterMotorController could not be found" ) ; } try { colorSensor = hardwareMap . colorSensor . get ( "colorSensor" ) ; colorSensor . enableLed ( true ) ; } catch ( Exception eColorSensor ) { colorSensor = new ModernRoboticsI2cColorSensor ( null , 0 ) ; telemetry . addData ( "Warning: " , "Color Sensor could not be found" ) ; } try { driveMotorController = hardwareMap . dcMotorController . get ( "DriveMotorController" ) ; } catch ( Exception eDriveController ) { driveMotorController = new DummyDcMotorController ( ) ; } try { gyro = ( ModernRoboticsI2cGyro ) hardwareMap . gyroSensor . get ( "gyro" ) ; } catch ( Exception eGyroSensor ) { gyro = null ; } try { pushRod = hardwareMap . crservo . get ( "pushRod" ) ; } catch ( Exception ePushRod ) { pushRod = new DummyCRServo ( ) ; } try { clawTrigger = hardwareMap . servo . get ( "clawTrigger" ) ; } catch ( Exception eClawTrigger ) { telemetry . addData ( "Exception " , "clawTrigger not found" ) ; clawTrigger = new DummyServo ( ) ; } 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 ( "Esception" , "opticalSenor not found" ) ; opticalDistanceSensor = new DummyOpticalDistanceSensor ( ) ; } if ( pushRod != null ) { pushRod . setPower ( - 1.0 ) ; } 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 ( ) ; cannonSeal . setPosition ( 0.0 ) ; switch ( currentStep ) { case 0 : driveStraight ( 0 ) ; if ( ! opModeIsActive ( ) ) ; break ; case 1 : shoot ( ) ; if ( ! opModeIsActive ( ) ) ; break ; case 2 : driveStraight ( 0 ) ; if ( ! opModeIsActive ( ) ) ; break ; case 3 : encodedgpsturn ( 0 ) ; if ( ! opModeIsActive ( ) ) ; break ; case 4 : driveStraight ( 0 ) ; if ( ! opModeIsActive ( ) ) ; break ; } stop ( ) ; }
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); //colorSensor = (ModernRoboticsI2cColorSensor) hardwareMap.colorSensor.get("colorSensor"); //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.