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

Example 1 with LynxResetMotorEncoderCommand

use of com.qualcomm.hardware.lynx.commands.core.LynxResetMotorEncoderCommand 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);
        }
    }
}