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

Example 1 with LynxSetMotorConstantPowerCommand

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