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

Example 1 with LynxSetMotorTargetVelocityCommand

use of com.qualcomm.hardware.lynx.commands.core.LynxSetMotorTargetVelocityCommand 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 LynxSetMotorTargetVelocityCommand

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