Examples with ElapsedTime com.qualcomm.robotcore.util.ElapsedTime used on opensource projects

Example 6 with ElapsedTime

use of com.qualcomm.robotcore.util.ElapsedTime in project Relic_Main by TeamOverdrive.

the class R_Back_3Glyph method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    waitForStart("RBack", AutoParams.AUTO, true);
    Robot.getDrive().zeroSensors();
    ElapsedTime t = new ElapsedTime();
    Robot.getJewel().hit(this.jewel_Color, Jewel_Color.Red);
    switch(RelicRecoveryVuMark.RIGHT) {
        case RIGHT:
            boolean rightFarther = false;
            Robot.getDrive().encoderDrive(0.6, 21, 21, 3);
            Robot.getDrive().encoderDrive(0.6, 0, (WHEEL_BASE * PI * (90)) / 180, 3);
            Robot.getDrive().encoderDrive(0.6, 2, 2, 3);
            Robot.getSlammer().stopperUp();
            sleep(400);
            Robot.getIntake().releaseIntake();
            Robot.getRelic().setAngles(0, 0);
            Robot.getRelic().setWristPostion(64);
            Robot.getRelic().close();
            Robot.getDrive().encoderDrive(0.7, -4, -4, 3);
            Robot.getSlammer().stopperDown();
            Robot.getIntake().intake();
            Robot.getDrive().encoderDrive(0.3, -20, -20, 3);
            Robot.getDrive().encoderDrive(0.2, -5, -5, 2);
            Robot.getRelic().lock();
            telemetry.clearAll();
            telemetry.addData("Front", Robot.getIntake().frontDetected());
            telemetry.addData("Back", Robot.getIntake().backDetected());
            telemetry.update();
            int counter = 0;
            for (int i = 0; i < 30; i++) {
                if ((Robot.getIntake().frontDetected() && Robot.getIntake().backDetected()))
                    counter++;
            }
            if (counter < 10) {
                rightFarther = true;
                sleep(400);
                Robot.getIntake().reverse();
                sleep(400);
                Robot.getIntake().intake();
                Robot.getDrive().encoderDrive(0.4, 3, 3, 3);
                Robot.getDrive().encoderDrive(0.4, -6, -6, 3);
                while (opModeIsActive() && t.seconds() < 18 && !(Robot.getIntake().frontDetected() && Robot.getIntake().backDetected())) {
                    Thread.yield();
                    telemetry.clearAll();
                    telemetry.addData("Front", Robot.getIntake().frontDetected());
                    telemetry.addData("Back", Robot.getIntake().backDetected());
                    telemetry.update();
                }
                Robot.getIntake().reverse();
                sleep(1000);
            } else {
                rightFarther = false;
                sleep(500);
                Robot.getIntake().reverse();
                sleep(1000);
            }
            Robot.getIntake().setPower(0.5);
            if (rightFarther) {
                Robot.getDrive().encoderDrive(0.4, (WHEEL_BASE * PI * (17)) / 180, 0, 3);
                Robot.getDrive().encoderDrive(0.4, 36, 36, 4);
            } else {
                Robot.getDrive().encoderDrive(0.4, (WHEEL_BASE * PI * (25)) / 180, 0, 3);
                Robot.getDrive().encoderDrive(0.4, 32, 32, 4);
            }
            Robot.getSlammer().autoSlam();
            Robot.getDrive().encoderDrive(0.5, -5, -5, 2);
            new Thread(new Runnable() {

                @Override
                public void run() {
                    Robot.getSlammer().setPower(-1);
                    sleep(300);
                    Robot.getSlammer().setPower(0);
                }
            }).start();
            if (30 - t.seconds() >= 4) {
                Robot.getDrive().encoderDrive(1, 4, 4, 2);
                Robot.getDrive().encoderDrive(1, -5, -5, 2);
            }
            break;
        case CENTER:
            boolean centerFarther = false;
            Robot.getDrive().encoderDrive(0.6, 22, 22, 4);
            Robot.getDrive().encoderDrive(0.6, 0, (WHEEL_BASE * PI * (57)) / 180, 3);
            Robot.getDrive().encoderDrive(0.6, 7, 7, 3);
            Robot.getSlammer().stopperUp();
            sleep(400);
            Robot.getIntake().releaseIntake();
            Robot.getDrive().encoderDrive(0.6, -(WHEEL_BASE * PI * (90 - 57)) / 180, 0, 3);
            Robot.getDrive().encoderDrive(0.6, -5, -5, 2);
            Robot.getSlammer().stopperDown();
            Robot.getIntake().intake();
            Robot.getRelic().setAngles(0, 0);
            Robot.getRelic().setWristPostion(64);
            Robot.getRelic().close();
            Robot.getDrive().encoderDrive(0.3, -20, -20, 3);
            Robot.getDrive().encoderDrive(0.2, -5, -5, 2);
            telemetry.clearAll();
            telemetry.addData("Front", Robot.getIntake().frontDetected());
            telemetry.addData("Back", Robot.getIntake().backDetected());
            telemetry.update();
            int counter2 = 0;
            for (int i = 0; i < 30; i++) {
                if ((Robot.getIntake().frontDetected() && Robot.getIntake().backDetected()))
                    counter2++;
            }
            if (counter2 < 10) {
                centerFarther = true;
                sleep(400);
                Robot.getIntake().reverse();
                sleep(400);
                Robot.getIntake().intake();
                Robot.getDrive().encoderDrive(0.4, 3, 3, 3);
                Robot.getDrive().encoderDrive(0.4, -6, -6, 3);
                while (opModeIsActive() && t.seconds() < 18 && !(Robot.getIntake().frontDetected() && Robot.getIntake().backDetected())) {
                    Thread.yield();
                    telemetry.clearAll();
                    telemetry.addData("Front", Robot.getIntake().frontDetected());
                    telemetry.addData("Back", Robot.getIntake().backDetected());
                    telemetry.update();
                }
                Robot.getIntake().reverse();
                sleep(1000);
            } else {
                centerFarther = false;
                sleep(500);
                Robot.getIntake().reverse();
                sleep(1000);
            }
            Robot.getIntake().intake();
            if (centerFarther) {
                Robot.getDrive().encoderDrive(0.4, 0, (WHEEL_BASE * PI * (-7)) / 180, 3);
                Robot.getDrive().encoderDrive(0.4, 39, 39, 4);
            } else {
                Robot.getDrive().encoderDrive(0.4, 0, (WHEEL_BASE * PI * (-3)) / 180, 3);
                Robot.getDrive().encoderDrive(0.4, 35, 35, 4);
            }
            telemetry.addData("Counter", counter2);
            telemetry.update();
            Robot.getSlammer().autoSlam();
            Robot.getDrive().encoderDrive(0.5, -5, -5, 2);
            new Thread(new Runnable() {

                @Override
                public void run() {
                    Robot.getSlammer().setPower(-1);
                    sleep(300);
                    Robot.getSlammer().setPower(0);
                }
            }).start();
            if (30 - t.seconds() >= 4) {
                Robot.getDrive().encoderDrive(1, 4, 4, 2);
                Robot.getDrive().encoderDrive(1, -5, -5, 2);
            }
            break;
        case LEFT:
            boolean leftFarther = false;
            Robot.getDrive().encoderDrive(0.6, 27, 27, 4);
            Robot.getDrive().encoderDrive(0.6, 0, (WHEEL_BASE * PI * (54)) / 180, 3);
            Robot.getDrive().encoderDrive(0.6, 7, 7, 3);
            Robot.getSlammer().stopperUp();
            sleep(400);
            Robot.getIntake().releaseIntake();
            Robot.getDrive().encoderDrive(0.6, -(WHEEL_BASE * PI * (90 - 54)) / 180, 0, 3);
            Robot.getRelic().setAngles(0, 0);
            Robot.getRelic().setWristPostion(64);
            Robot.getRelic().close();
            Robot.getDrive().encoderDrive(0.6, -5, -5, 2);
            Robot.getSlammer().stopperDown();
            Robot.getIntake().intake();
            Robot.getDrive().encoderDrive(0.3, -20, -20, 3);
            Robot.getDrive().encoderDrive(0.2, -5, -5, 2);
            Robot.getRelic().lock();
            telemetry.clearAll();
            telemetry.addData("Front", Robot.getIntake().frontDetected());
            telemetry.addData("Back", Robot.getIntake().backDetected());
            telemetry.update();
            if (!(Robot.getIntake().frontDetected() && Robot.getIntake().backDetected())) {
                leftFarther = true;
                sleep(400);
                Robot.getIntake().reverse();
                sleep(400);
                Robot.getIntake().intake();
                Robot.getDrive().encoderDrive(0.4, 3, 3, 3);
                Robot.getDrive().encoderDrive(0.4, -6, -6, 3);
                while (opModeIsActive() && t.seconds() < 18 && !(Robot.getIntake().frontDetected() && Robot.getIntake().backDetected())) {
                    Thread.yield();
                    telemetry.clearAll();
                    telemetry.addData("Front", Robot.getIntake().frontDetected());
                    telemetry.addData("Back", Robot.getIntake().backDetected());
                    telemetry.update();
                }
                Robot.getIntake().reverse();
                sleep(500);
            } else {
                leftFarther = false;
                sleep(500);
                Robot.getIntake().reverse();
                sleep(300);
            }
            Robot.getIntake().setPower(0.5);
            if (leftFarther) {
                Robot.getDrive().encoderDrive(0.4, 0, (WHEEL_BASE * PI * (9)) / 180, 3);
                Robot.getDrive().encoderDrive(0.4, 39, 39, 4);
            } else {
                Robot.getDrive().encoderDrive(0.4, 0, (WHEEL_BASE * PI * (10)) / 180, 3);
                Robot.getDrive().encoderDrive(0.4, 35, 35, 4);
            }
            Robot.getSlammer().autoSlam();
            Robot.getDrive().encoderDrive(0.5, -5, -5, 2);
            new Thread(new Runnable() {

                @Override
                public void run() {
                    Robot.getSlammer().setPower(-1);
                    sleep(300);
                    Robot.getSlammer().setPower(0);
                }
            }).start();
            if (30 - t.seconds() >= 4) {
                Robot.getDrive().encoderDrive(1, 4, 4, 2);
                Robot.getDrive().encoderDrive(1, -5, -5, 2);
            }
            break;
    }
}

Example 7 with ElapsedTime

use of com.qualcomm.robotcore.util.ElapsedTime in project Relic_Main by TeamOverdrive.

the class B_Back_3Glyph method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    waitForStart("BBack", AutoParams.AUTO, true);
    Robot.getDrive().zeroSensors();
    ElapsedTime t = new ElapsedTime();
    Robot.getJewel().hit(this.jewel_Color, Jewel_Color.Blue);
    Robot.getDrive().encoderDrive(0.3, -36, -36, 4);
    Robot.getDrive().encoderDrive(0.3, 0, (WHEEL_BASE * PI * (82)) / 180, 4);
    Robot.getDrive().encoderDrive(0.3, 7, 7, 3);
    Robot.getSlammer().stopperUp();
    sleep(600);
    Robot.getDrive().encoderDrive(0.3, -10, -10, 3);
// getDrive().turnCW(56, 0.3, 3);
// getDrive().encoderDrive(0.3, 7, 7, 3);
// getSlammer().stopperUp();
// sleep(600);
// getDrive().encoderDrive(0.3, -7, -7, 4);
// getSlammer().stopperDown();
// getIntake().releaseIntake();
// getDrive().turnCW(98-56, 0.6, 3);
// //getDrive().encoderDrive(0.6, -(WHEEL_BASE*PI*(90-70))/180, 0, 3);
// 
// getDrive().encoderDrive(0.6, -5, -5, 4);
// 
// getSlammer().stopperDown();
// getIntake().intake();
// getRelic().setWristPostion(50);
// getRelic().setAngles(0,0);
// 
// getIntake().reverse();
// new Thread(new Runnable() {
// @Override
// public void run() {
// sleep(500);
// getIntake().intake();
// }
// }).start();
// 
// getDrive().encoderDrive(0.3, -25, -25, 4);
// 
// while (opModeIsActive() && t.seconds()<16 && !(getIntake().frontDetected() || getIntake().backDetected())){
// Thread.yield();
// telemetry.clearAll();
// telemetry.addData("Front", getIntake().frontDetected());
// telemetry.addData("Back", getIntake().backDetected());
// telemetry.update();
// }
}

Example 8 with ElapsedTime

use of com.qualcomm.robotcore.util.ElapsedTime in project robotcode by OutoftheBoxFTC.

the class ReadWriteRunnableStandard method run.

/**
 * Main read / write loop
 */
@Override
public void run() {
    // Paranoia: avoid re-execution, as we weren't designed for that
    if (shutdownComplete) {
        return;
    }
    ThreadPool.logThreadLifeCycle(String.format("r/w loop: %s", HardwareFactory.getDeviceDisplayName(context, serialNumber)), new Runnable() {

        @Override
        public void run() {
            // read the entire buffer at least one time.
            fullWriteNeeded = false;
            pruneBufferAfterRead = true;
            setFullActive();
            ElapsedTime timer = new ElapsedTime();
            String timerString = "Device " + serialNumber.toString();
            // Tell those awaiting our startup that we are up and running
            running = true;
            try {
                callback.startupComplete();
            } catch (InterruptedException e) {
                // shutdown, in the normal way, soon!
                running = false;
            }
            runningInterlock.countDown();
            try {
                while (running) {
                    if (debugLogging) {
                        timer.log(timerString);
                        timer.reset();
                    }
                    doReadCycle();
                    doWriteCycle();
                    usbHandler.throwIfTooManySequentialCommErrors();
                    if (!robotUsbDevice.isOpen()) {
                        throw new RobotUsbDeviceClosedException("%s: closed", robotUsbDevice.getSerialNumber());
                    }
                }
            // end loop
            } catch (InterruptedException e) {
                RobotLog.logExceptionHeader(TAG, e, "thread interrupt while communicating with %s", HardwareFactory.getDeviceDisplayName(context, serialNumber));
            // Shutting down due to an interrupt is NOT abnormal: this may well happen when an invocation
            // of our close() method requests an interrupt from the FTDI layer.
            } catch (RobotUsbException | RuntimeException e) {
                // than something we should pollute the logs with
                if (e.getClass() != RobotUsbDeviceClosedException.class) {
                    RobotLog.ee(TAG, e, "exception while communicating with %s", HardwareFactory.getDeviceDisplayName(context, serialNumber));
                }
                String format = context.getString(robotUsbDevice.isAttached() ? R.string.warningProblemCommunicatingWithUSBDevice : R.string.warningUSBDeviceDetached);
                setOwnerWarningMessage(format, HardwareFactory.getDeviceDisplayName(context, serialNumber));
                // cases.
                if (e.getClass() == RobotUsbTimeoutException.class || e.getClass() == RobotUsbStuckUsbWriteException.class) {
                    shutdownReason = ShutdownReason.ABNORMAL_ATTEMPT_REOPEN;
                } else {
                    shutdownReason = ShutdownReason.ABNORMAL;
                }
            } finally {
                usbHandler.close();
                running = false;
                try {
                    callback.shutdownComplete();
                } catch (InterruptedException e) {
                // ignore: we're about to terminate this thread anyway
                }
                shutdownComplete = true;
            }
        }
    });
}

Example 9 with ElapsedTime

use of com.qualcomm.robotcore.util.ElapsedTime in project robotcode by OutoftheBoxFTC.

the class EventLoopManager method startEventLoop.

private void startEventLoop() throws RobotCoreException {
    // call the init method
    try {
        changeState(RobotState.INIT);
        synchronized (eventLoopLock) {
            eventLoop.init(this);
        }
    } catch (Exception e) {
        RobotLog.ww(TAG, e, "Caught exception during looper init: " + e.toString());
        changeState(RobotState.EMERGENCY_STOP);
        this.refreshSystemTelemetry();
        throw new RobotCoreException("Robot failed to start: " + e.getMessage());
    }
    // reset the heartbeat timer
    lastHeartbeatReceived = new ElapsedTime(0);
    // start the new event loop
    changeState(RobotState.RUNNING);
    executorEventLoop = ThreadPool.newSingleThreadExecutor("executorEventLoop");
    executorEventLoop.execute(new Runnable() {

        @Override
        public void run() {
            // Run the main event loop
            new EventLoopRunnable().run();
            // When that terminates (perhaps due to an exception), run the idle loop until we're asked
            // to shutdown. This keeps *some* event loop always running so long as we're not shutdown.
            eventLoop = idleEventLoop;
            boolean runIdle = true;
            if (!Thread.currentThread().isInterrupted()) {
                RobotLog.vv(TAG, "switching to idleEventLoop");
                try {
                    synchronized (eventLoopLock) {
                        eventLoop.init(EventLoopManager.this);
                    }
                } catch (InterruptedException e) {
                    Thread.currentThread().interrupt();
                } catch (RobotCoreException e) {
                    RobotLog.ee(TAG, e, "internal error");
                    runIdle = false;
                }
                if (runIdle) {
                    new EventLoopRunnable().run();
                }
            }
        }
    });
}

Example 10 with ElapsedTime

use of com.qualcomm.robotcore.util.ElapsedTime in project robotcode by OutoftheBoxFTC.

the class EventLoopManager method peerDisconnected.

@Override
public void peerDisconnected() {
    if (callback != null) {
        callback.onPeerDisconnected();
    }
    if (this.isPeerConnected) {
        // If we lose contact with the DS, then we auto-stop the robot
        OpModeManagerImpl opModeManager = eventLoop.getOpModeManager();
        // opModeManager will be null if not running FtcEventLoop right now
        if (opModeManager != null) {
            String msg = "Lost connection while running op mode: " + opModeManager.getActiveOpModeName();
            opModeManager.initActiveOpMode(OpModeManager.DEFAULT_OP_MODE_NAME);
            RobotLog.i(msg);
        } else {
            RobotLog.i("Lost connection while main event loop not active");
        }
        this.isPeerConnected = false;
    }
    remoteAddr = null;
    lastHeartbeatReceived = new ElapsedTime(0);
}