public void setSpeed(OutputStream out, byte[] buf, int len) { int leftSign = 1; int rightSign = 1; int p = 2; if (buf[p] == '-') { // if input is '-', move right one place leftSign = -1; p++; } int leftPwr = 0; while ('0' <= buf[p] && buf[p] <= '9') leftPwr = leftPwr * 10 + (buf[p++] - '0'); // accumulate the decimal value if (leftPwr > H_MAX_POWER) leftPwr = H_MAX_POWER; // stay in range leftPwr = leftSign * leftPwr; // assert(buf[p] == ','); p++; if (buf[p] == '-') { rightSign = -1; p++; } int rightPwr = 0; while ('0' <= buf[p] && buf[p] <= '9') rightPwr = rightPwr * 10 + (buf[p++] - '0'); if (rightPwr > H_MAX_POWER) rightPwr = H_MAX_POWER; // stay in range rightPwr = rightSign * rightPwr; // assert(buf[p] == 0); // the buffer is null-terminated p++; leftMotor.setPower((leftPwr * 16) / H_MAX_POWER); rightMotor.setPower((rightPwr * 16) / H_MAX_POWER); writeCRLF(out, "d"); }
public void motorsOff(OutputStream out, byte[] buf, int len) { leftMotor.setPower(0); rightMotor.setPower(0); writeCRLF(out, "z"); }