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");
}
