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

Example 11 with ElapsedTime

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

the class BNO055IMUImpl method internalInitializeOnce.

/**
 * Do one attempt at initializing the device to be running in the indicated operation mode
 */
protected boolean internalInitializeOnce(SystemStatus expectedStatus) {
    // Validate parameters
    if (SensorMode.CONFIG == parameters.mode) {
        throw new IllegalArgumentException("SensorMode.CONFIG illegal for use as initialization mode");
    }
    ElapsedTime elapsed = new ElapsedTime();
    if (parameters.accelerationIntegrationAlgorithm != null) {
        this.accelerationAlgorithm = parameters.accelerationIntegrationAlgorithm;
    }
    // Lore: "send a throw-away command [...] just to make sure the BNO is in a good state
    // and ready to accept commands (this seems to be necessary after a hard power down)."
    write8(Register.PAGE_ID, 0);
    // Make sure we have the right device
    byte chipId = read8(Register.CHIP_ID);
    if (chipId != bCHIP_ID_VALUE) {
        // delay value is from from Table 0-2 in the BNO055 specification
        delayExtra(650);
        chipId = read8(Register.CHIP_ID);
        if (chipId != bCHIP_ID_VALUE) {
            log_e("unexpected chip: expected=%d found=%d", bCHIP_ID_VALUE, chipId);
            return false;
        }
    }
    // Get us into config mode, for sure
    setSensorMode(SensorMode.CONFIG);
    // Reset the system, and wait for the chip id register to switch back from its reset state
    // to the it's chip id state. This can take a very long time, some 650ms (Table 0-2, p13)
    // perhaps. While in the reset state the chip id (and other registers) reads as 0xFF.
    TimestampedI2cData.suppressNewHealthWarnings(true);
    try {
        elapsed.reset();
        write8(Register.SYS_TRIGGER, 0x20);
        for (; ; ) {
            chipId = read8(Register.CHIP_ID);
            if (chipId == bCHIP_ID_VALUE) {
                break;
            }
            delayExtra(10);
            if (elapsed.milliseconds() > msAwaitChipId) {
                log_e("failed to retrieve chip id");
                return false;
            }
        }
        delayLoreExtra(50);
    } finally {
        TimestampedI2cData.suppressNewHealthWarnings(false);
    }
    // Set to normal power mode
    write8(Register.PWR_MODE, POWER_MODE.NORMAL.getValue());
    delayLoreExtra(10);
    // Make sure we're looking at register page zero, as the other registers
    // we need to set here are on that page.
    write8(Register.PAGE_ID, 0);
    // Set the output units. Section 3.6, p31
    int unitsel = // pitch angle convention
    (parameters.pitchMode.bVal << 7) | // temperature
    (parameters.temperatureUnit.bVal << 4) | // euler angle units
    (parameters.angleUnit.bVal << 2) | // gyro units, per second
    (parameters.angleUnit.bVal << 1) | // accelerometer units
    (parameters.accelUnit.bVal);
    write8(Register.UNIT_SEL, unitsel);
    // Use or don't use the external crystal
    // See Section 5.5 (p100) of the BNO055 specification.
    write8(Register.SYS_TRIGGER, parameters.useExternalCrystal ? 0x80 : 0x00);
    delayLoreExtra(50);
    // Switch to page 1 so we can write some more registers
    write8(Register.PAGE_ID, 1);
    // Configure selected page 1 registers
    write8(Register.ACC_CONFIG, parameters.accelPowerMode.bVal | parameters.accelBandwidth.bVal | parameters.accelRange.bVal);
    write8(Register.MAG_CONFIG, parameters.magPowerMode.bVal | parameters.magOpMode.bVal | parameters.magRate.bVal);
    write8(Register.GYR_CONFIG_0, parameters.gyroBandwidth.bVal | parameters.gyroRange.bVal);
    write8(Register.GYR_CONFIG_1, parameters.gyroPowerMode.bVal);
    // Switch back
    write8(Register.PAGE_ID, 0);
    // Run a self test. This appears to be a necessary step in order for the
    // sensor to be able to actually be used. That is, we've observed that absent this,
    // the sensors do not return correct data. We wish that were documented somewhere.
    // SYS_TRIGGER=0x3F
    write8(Register.SYS_TRIGGER, read8(Register.SYS_TRIGGER) | 0x01);
    // Start a timer: we only give the self-test a certain length of time to run
    elapsed.reset();
    // It's a little unclear how to conclude when the self test is complete. getSystemStatus()
    // can report SystemStatus.SELF_TEST, and one might be lead to think that that will remain
    // true while the self test is running, but that appears not actually to be the case, as
    // sometimes we see SystemStatus.SELF_TEST being reported even after two full seconds. So,
    // we fall back on to what we've always done, and just check the results of the tested
    // sensors we actually care about.
    // Per Section 3.9.2 Built In Self Test, when we manually kick off a self test,
    // the accelerometer, gyro, and magnetometer are tested, but the microcontroller is not.
    // So: we only look for successful results from those three.
    final int successfulResult = 0x07;
    final int successfulResultMask = 0x07;
    boolean selfTestSuccessful = false;
    while (!selfTestSuccessful && elapsed.milliseconds() < msAwaitSelfTest) {
        // SELFTEST_RESULT=0x36
        selfTestSuccessful = (read8(Register.SELFTEST_RESULT) & successfulResultMask) == successfulResult;
    }
    if (!selfTestSuccessful) {
        int result = read8(Register.SELFTEST_RESULT);
        log_e("self test failed: 0x%02x", result);
        return false;
    }
    if (this.parameters.calibrationData != null) {
        writeCalibrationData(this.parameters.calibrationData);
    } else if (this.parameters.calibrationDataFile != null) {
        try {
            File file = AppUtil.getInstance().getSettingsFile(this.parameters.calibrationDataFile);
            String serialized = ReadWriteFile.readFileOrThrow(file);
            CalibrationData data = CalibrationData.deserialize(serialized);
            writeCalibrationData(data);
        } catch (IOException e) {
        // Ignore the absence of the indicated file, etc
        }
    }
    // Finally, enter the requested operating mode (see section 3.3).
    setSensorMode(parameters.mode);
    // At this point, the chip should in fact report correctly that it's in the mode requested.
    // See Section '4.3.58 SYS_STATUS' of the BNO055 specification. That said, we've seen issues
    // where the first mode request somehow doesn't take, so we re-issue. We don't understand the
    // circumstances that cause this condition (or we'd avoid them!).
    SystemStatus status = getSystemStatus();
    if (status != expectedStatus) {
        log_w("re-issuing IMU mode: system status=%s expected=%s", status, expectedStatus);
        delayLore(100);
        setSensorMode(parameters.mode);
        status = getSystemStatus();
    }
    if (status == expectedStatus) {
        return true;
    } else {
        log_w("IMU initialization failed: system status=%s expected=%s", status, expectedStatus);
        return false;
    }
}

Example 12 with ElapsedTime

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

the class ConceptTelemetry method runOpMode.

@Override
public void runOpMode() {
    /* we keep track of how long it's been since the OpMode was started, just
         * to have some interesting data to show */
    ElapsedTime opmodeRunTime = new ElapsedTime();
    // We show the log in oldest-to-newest order, as that's better for poetry
    telemetry.log().setDisplayOrder(Telemetry.Log.DisplayOrder.OLDEST_FIRST);
    // We can control the number of lines shown in the log
    telemetry.log().setCapacity(6);
    // The interval between lines of poetry, in seconds
    double sPoemInterval = 0.6;
    /**
     * Wait until we've been given the ok to go. For something to do, we emit the
     * elapsed time as we sit here and wait. If we didn't want to do anything while
     * we waited, we would just call {@link #waitForStart()}.
     */
    while (!isStarted()) {
        telemetry.addData("time", "%.1f seconds", opmodeRunTime.seconds());
        telemetry.update();
        idle();
    }
    // Ok, we've been given the ok to go
    /**
     * As an illustration, the first line on our telemetry display will display the battery voltage.
     * The idea here is that it's expensive to compute the voltage (at least for purposes of illustration)
     * so you don't want to do it unless the data is <em>actually</em> going to make it to the
     * driver station (recall that telemetry transmission is throttled to reduce bandwidth use.
     * Note that getBatteryVoltage() below returns 'Infinity' if there's no voltage sensor attached.
     *
     * @see Telemetry#getMsTransmissionInterval()
     */
    telemetry.addData("voltage", "%.1f volts", new Func<Double>() {

        @Override
        public Double value() {
            return getBatteryVoltage();
        }
    });
    // Reset to keep some timing stats for the post-'start' part of the opmode
    opmodeRunTime.reset();
    int loopCount = 1;
    // Go go gadget robot!
    while (opModeIsActive()) {
        // Emit poetry if it's been a while
        if (poemElapsed.seconds() > sPoemInterval) {
            emitPoemLine();
        }
        // As an illustration, show some loop timing information
        telemetry.addData("loop count", loopCount);
        telemetry.addData("ms/loop", "%.3f ms", opmodeRunTime.milliseconds() / loopCount);
        // Show joystick information as some other illustrative data
        telemetry.addLine("left joystick | ").addData("x", gamepad1.left_stick_x).addData("y", gamepad1.left_stick_y);
        telemetry.addLine("right joystick | ").addData("x", gamepad1.right_stick_x).addData("y", gamepad1.right_stick_y);
        /**
         * Transmit the telemetry to the driver station, subject to throttling.
         * @see Telemetry#getMsTransmissionInterval()
         */
        telemetry.update();
        /**
         * Update loop info and play nice with the rest of the {@link Thread}s in the system
         */
        loopCount++;
    }
}

Example 13 with ElapsedTime

use of com.qualcomm.robotcore.util.ElapsedTime in project FTC-2017 by FIRST-4030.

the class ConceptTelemetry method runOpMode.

@Override
public void runOpMode() {
    /* we keep track of how long it's been since the OpMode was started, just
         * to have some interesting data to show */
    ElapsedTime opmodeRunTime = new ElapsedTime();
    // We show the log in oldest-to-newest order, as that's better for poetry
    telemetry.log().setDisplayOrder(Telemetry.Log.DisplayOrder.OLDEST_FIRST);
    // We can control the number of lines shown in the log
    telemetry.log().setCapacity(6);
    // The interval between lines of poetry, in seconds
    double sPoemInterval = 0.6;
    /**
     * Wait until we've been given the ok to go. For something to do, we emit the
     * elapsed time as we sit here and wait. If we didn't want to do anything while
     * we waited, we would just call {@link #waitForStart()}.
     */
    while (!isStarted()) {
        telemetry.addData("time", "%.1f seconds", opmodeRunTime.seconds());
        telemetry.update();
        idle();
    }
    // Ok, we've been given the ok to go
    /**
     * As an illustration, the first line on our telemetry display will display the battery voltage.
     * The idea here is that it's expensive to compute the voltage (at least for purposes of illustration)
     * so you don't want to do it unless the data is <em>actually</em> going to make it to the
     * driver station (recall that telemetry transmission is throttled to reduce bandwidth use.
     * Note that getBatteryVoltage() below returns 'Infinity' if there's no voltage sensor attached.
     *
     * @see Telemetry#getMsTransmissionInterval()
     */
    telemetry.addData("voltage", "%.1f volts", new Func<Double>() {

        @Override
        public Double value() {
            return getBatteryVoltage();
        }
    });
    // Reset to keep some timing stats for the post-'start' part of the opmode
    opmodeRunTime.reset();
    int loopCount = 1;
    // Go go gadget robot!
    while (opModeIsActive()) {
        // Emit poetry if it's been a while
        if (poemElapsed.seconds() > sPoemInterval) {
            emitPoemLine();
        }
        // As an illustration, show some loop timing information
        telemetry.addData("loop count", loopCount);
        telemetry.addData("ms/loop", "%.3f ms", opmodeRunTime.milliseconds() / loopCount);
        // Show joystick information as some other illustrative data
        telemetry.addLine("left joystick | ").addData("x", gamepad1.left_stick_x).addData("y", gamepad1.left_stick_y);
        telemetry.addLine("right joystick | ").addData("x", gamepad1.right_stick_x).addData("y", gamepad1.right_stick_y);
        /**
         * Transmit the telemetry to the driver station, subject to throttling.
         * @see Telemetry#getMsTransmissionInterval()
         */
        telemetry.update();
        /**
         * Update loop info and play nice with the rest of the {@link Thread}s in the system
         */
        loopCount++;
    }
}

Example 14 with ElapsedTime

use of com.qualcomm.robotcore.util.ElapsedTime in project FTC-2017 by FIRST-4030.

the class PushbotAutoDriveByGyro_Linear method gyroHold.

/**
 *  Method to obtain & hold a heading for a finite amount of time
 *  Move will stop once the requested time has elapsed
 *
 * @param speed      Desired speed of turn.
 * @param angle      Absolute Angle (in Degrees) relative to last gyro reset.
 *                   0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
 *                   If a relative angle is required, add/subtract from current heading.
 * @param holdTime   Length of time (in seconds) to hold the specified heading.
 */
public void gyroHold(double speed, double angle, double holdTime) {
    ElapsedTime holdTimer = new ElapsedTime();
    // keep looping while we have time remaining.
    holdTimer.reset();
    while (opModeIsActive() && (holdTimer.time() < holdTime)) {
        // Update telemetry & Allow time for other processes to run.
        onHeading(speed, angle, P_TURN_COEFF);
        telemetry.update();
    }
    // Stop all motion;
    robot.leftDrive.setPower(0);
    robot.rightDrive.setPower(0);
}

Example 15 with ElapsedTime

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

the class R_Back_5Glyph 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);
    Robot.getDrive().encoderDrive(0.5, 34.5, 34.5, 3);
    Robot.getDrive().turnCW(90, 0.5, 3);
    Robot.getDrive().encoderDrive(0.7, 8, 8, 2);
    Robot.getSlammer().stopperUp();
    sleep(400);
    Robot.getIntake().releaseIntake();
    Robot.getDrive().encoderDrive(1, -8, -8, 1.6);
    Robot.getIntake().intake();
    Robot.getRelic().setAngles(0, 0);
    Robot.getSlammer().stopperDown();
    sleep(200);
    Robot.getDrive().encoderDrive(0.4, -28, -28, 3);
    Robot.getIntake().reverse();
    sleep(400);
    boolean farther = false;
    if (!Robot.getIntake().frontDetected() && !Robot.getIntake().backDetected()) {
        Robot.getIntake().intake();
        Robot.getDrive().encoderDrive(0.2, -7, -7, 3);
        Robot.getIntake().reverse();
        sleep(1000);
        farther = true;
    }
    Robot.getIntake().intake();
    // getDrive().encoderDrive(1, 0, (WHEEL_BASE*PI*7)/180, 2);
    Robot.getDrive().encoderDrive(1, 37, 37, 3);
    Robot.getSlammer().autoSlam();
    Robot.getDrive().encoderDrive(1, -5, -5, 2);
    Robot.getSlammer().setPower(-1);
    sleep(1000);
    Robot.getSlammer().stop();
    Robot.getSlammer().stopperDown();
    Robot.getIntake().intake();
    Robot.getDrive().encoderDrive(1, -37, -37, 3);
    Robot.getDrive().encoderDrive(1, (WHEEL_BASE * PI * 9) / 180, 0, 2);
    Robot.getDrive().encoderDrive(1, 40, 40, 3);
    Robot.getSlammer().autoSlam();
    Robot.getDrive().encoderDrive(1, -4, -4, 2);
}