Examples with LynxCommand com.qualcomm.hardware.lynx.commands.LynxCommand used on opensource projects

Example 1 with LynxCommand

use of com.qualcomm.hardware.lynx.commands.LynxCommand in project robotcode by OutoftheBoxFTC.

the class LynxDcMotorController method internalSetMotorPower.

void internalSetMotorPower(int motorZ, double apiPower, boolean forceUpdate) {
    double power = Range.clip(apiPower, apiPowerFirst, apiPowerLast);
    int iPower = 0;
    if (motors[motorZ].lastKnownPower.updateValue(power) || forceUpdate) {
        DcMotor.RunMode mode = internalGetPublicMotorMode(motorZ);
        LynxCommand command = null;
        switch(mode) {
            case RUN_TO_POSITION:
            case RUN_USING_ENCODER:
                {
                    // Scale 'power' to configured maximum motor speed. This is mostly for legacy
                    // compatibility, as setMotorVelocity exposes this more directly.
                    power = Math.signum(power) * Range.scale(Math.abs(power), 0, apiPowerLast, 0, getDefaultMaxMotorSpeed(motorZ));
                    iPower = (int) power;
                    command = new LynxSetMotorTargetVelocityCommand(this.getModule(), motorZ, iPower);
                    break;
                }
            case RUN_WITHOUT_ENCODER:
                {
                    power = Range.scale(power, apiPowerFirst, apiPowerLast, LynxSetMotorConstantPowerCommand.apiPowerFirst, LynxSetMotorConstantPowerCommand.apiPowerLast);
                    iPower = (int) power;
                    command = new LynxSetMotorConstantPowerCommand(this.getModule(), motorZ, iPower);
                    break;
                }
            case STOP_AND_RESET_ENCODER:
                {
                    // Setting motor power in this mode doesn't do anything
                    command = null;
                    break;
                }
        }
        try {
            if (command != null) {
                if (DEBUG) {
                    RobotLog.vv(TAG, "setMotorPower: mod=%d motor=%d iPower=%d", getModuleAddress(), motorZ, iPower);
                }
                command.send();
                internalSetMotorEnable(motorZ, true);
            }
        } catch (InterruptedException | RuntimeException | LynxNackException e) {
            handleException(e);
        }
    }
}

Example 2 with LynxCommand

use of com.qualcomm.hardware.lynx.commands.LynxCommand in project robotcode by OutoftheBoxFTC.

the class LynxDcMotorController method internalSetZeroPowerBehavior.

void internalSetZeroPowerBehavior(int motorZ, DcMotor.ZeroPowerBehavior behavior) {
    if (motors[motorZ].lastKnownZeroPowerBehavior.updateValue(behavior)) {
        DcMotor.RunMode runMode = internalGetMotorChannelMode(motorZ);
        LynxCommand command = new LynxSetMotorChannelModeCommand(this.getModule(), motorZ, runMode, behavior);
        try {
            if (DEBUG) {
                RobotLog.vv(TAG, "setZeroBehavior mod=%d motor=%d zero=%s", getModuleAddress(), motorZ, behavior.toString());
            }
            command.send();
        } catch (InterruptedException | RuntimeException | LynxNackException e) {
            handleException(e);
        }
    }
}

Example 3 with LynxCommand

use of com.qualcomm.hardware.lynx.commands.LynxCommand in project robotcode by OutoftheBoxFTC.

the class LynxDcMotorController method setMotorVelocity.

@Override
public synchronized void setMotorVelocity(int motor, double angularRate, AngleUnit unit) {
    // If we're setting a target velocity, then we have to be in velocity mode, so put us there
    // if we aren't already (the alternative would have been to throw an error, which seems pointless).
    setMotorMode(motor, DcMotor.RunMode.RUN_USING_ENCODER);
    this.validateMotor(motor);
    motor -= apiMotorFirst;
    double degreesPerSecond = UnnormalizedAngleUnit.DEGREES.fromUnit(unit.getUnnormalized(), angularRate);
    double revolutionsPerSecond = degreesPerSecond / 360.0;
    double ticksPerSecond = motors[motor].motorType.getTicksPerRev() * revolutionsPerSecond;
    int iTicksPerSecond = Range.clip((int) Math.round(ticksPerSecond), LynxSetMotorTargetVelocityCommand.apiVelocityFirst, LynxSetMotorTargetVelocityCommand.apiVelocityLast);
    try {
        LynxCommand command = new LynxSetMotorTargetVelocityCommand(this.getModule(), motor, iTicksPerSecond);
        if (DEBUG) {
            RobotLog.vv(TAG, "setMotorVelocity: mod=%d motor=%d iPower=%d", getModuleAddress(), motor, iTicksPerSecond);
        }
        command.send();
        internalSetMotorEnable(motor, true);
    } catch (InterruptedException | RuntimeException | LynxNackException e) {
        handleException(e);
    }
}

Example 4 with LynxCommand

use of com.qualcomm.hardware.lynx.commands.LynxCommand in project robotcode by OutoftheBoxFTC.

the class LynxDcMotorController method setMotorMode.

@Override
public synchronized void setMotorMode(int motor, DcMotor.RunMode mode) {
    this.validateMotor(motor);
    motor -= apiMotorFirst;
    // and power levels will be interpreted correctly.
    if (!motors[motor].lastKnownMode.isValue(mode)) {
        // Get the current power so we can preserve across the change.
        Double prevPower = motors[motor].lastKnownPower.getNonTimedValue();
        if (prevPower == null) {
            prevPower = internalGetMotorPower(motor);
        }
        LynxCommand command;
        DcMotor.ZeroPowerBehavior zeroPowerBehavior = DcMotor.ZeroPowerBehavior.UNKNOWN;
        if (mode == DcMotor.RunMode.STOP_AND_RESET_ENCODER) {
            // Stop the motor, but not in such a way that we disrupt the last known
            // power, since we need to restore same when we come out of this mode.
            internalSetMotorPower(motor, 0);
            command = new LynxResetMotorEncoderCommand(this.getModule(), motor);
        } else {
            zeroPowerBehavior = internalGetZeroPowerBehavior(motor);
            command = new LynxSetMotorChannelModeCommand(this.getModule(), motor, mode, zeroPowerBehavior);
        }
        try {
            if (DEBUG) {
                RobotLog.vv(TAG, "setMotorChannelMode: mod=%d motor=%d mode=%s power=%f zero=%s", getModuleAddress(), motor, mode.toString(), prevPower, zeroPowerBehavior.toString());
            }
            command.send();
            // Ok, remember that mode. Note we need to set it before we call internalSetMotorPower()
            motors[motor].lastKnownMode.setValue(mode);
            // re-issue current motor power to ensure it's correct for this mode
            internalSetMotorPower(motor, prevPower, true);
        } catch (InterruptedException | RuntimeException | LynxNackException e) {
            handleException(e);
        }
    }
}

Example 5 with LynxCommand

use of com.qualcomm.hardware.lynx.commands.LynxCommand in project robotcode by OutoftheBoxFTC.

the class LynxI2cDeviceSynchV2 method readTimeStamped.

/*
     * Supporting firmware version XX.XX.XX
     */
@Override
public synchronized TimestampedData readTimeStamped(final int ireg, final int creg) {
    LynxI2cDeviceSynchV2 deviceHavingProblems = null;
    boolean reportUnhealthy;
    try {
        return acquireI2cLockWhile(new Supplier<TimestampedData>() {

            @Override
            public TimestampedData get() throws InterruptedException, RobotCoreException, LynxNackException {
                LynxCommand<?> tx = new LynxI2cWriteReadMultipleBytesCommand(getModule(), bus, i2cAddr, ireg, creg);
                tx.send();
                readTimeStampedPlaceholder.reset();
                return pollForReadResult(i2cAddr, ireg, creg);
            }
        });
    } catch (InterruptedException | RobotCoreException | RuntimeException e) {
        handleException(e);
    } catch (LynxNackException e) {
        /*
             * This is a possible device problem, go ahead and tell makeFakeData to warn.
             */
        deviceHavingProblems = this;
        handleException(e);
    }
    return readTimeStampedPlaceholder.log(TimestampedI2cData.makeFakeData(deviceHavingProblems, getI2cAddress(), ireg, creg));
}