void Robot::turnLeft(float angle)
{
ori -= angle*M_PI/180;
ori = ori>2*M_PI?ori-2*M_PI:ori;
ori = ori<0?ori+2*M_PI:ori;
motor_turnLeft((angle+TURN_LEFT_OFFSET*angle/30.0)*M_PI/180);
updateRadar();
}
void loop() {
if (Serial.available()) {
String readString = "";
while (Serial.available()) {
delay(3);
if (Serial.available() > 0) {
char c = Serial.read();
readString += c;
}
}
if (readString.charAt(0) == 'M') {
switch (readString.charAt(1)) {
case 'f' : case 'F':
motor_forward();
break;
case 'b': case 'B':
motor_backward();
break;
case 'l': case 'L':
motor_turnLeft();
break;
case 'r': case 'R':
motor_turnRight();
break;
case 's': case 'S':
motor_stop();
break;
case 'm': case 'A':
isUltra = !isUltra;
break;
case 'p': case 'P':
char c = readString.charAt(2);
ChangeSpeed((c - '0') * factor + minSpeed);
break;
}
}
} else {
if (isUltra) {
//ultraCarProcess();
}
else {
//motor_stop();
}
}
}
