Ошибок нет, но код не работает
Помогите, пожалуйста! При наборе в терминал команд например A158, рука (servoPin_Arm) должна повернуться в положение 158 градусов, а в терминал вернуться "Moving 158 degrees". Увы! Код не работает. При наборе команд в терминал ничего не возвращается, а сервопривод стоит на месте. Ошибок я не нашел, код компилируется. Почему не работает и что делать?
const int servoPin_Arm = 10; // сигнальный провод от серво на порт 10
const int servoPin_Torso = 11; // сигнальный провод от серво на порт 9
const int servoPin_Body = 9; // сигнальный провод от серво на порт 8
const int servoPin_Hand = 6; // сигнальный провод от серво на порт 7
int tmp, val;
int flag_recv_1st_sym = 0;
//makros
#define TURN_ARM servoPulse(servoPin_Arm, val)
#define TURN_TORSO servoPulse (servoPin_Torso, val)
#define TURN_BODY servoPulse (servoPin_Body, val)
#define TURN_HAND servoPulse (servoPin_Hand, val)
void setup()
{
pinMode(servoPin_Torso, OUTPUT);
pinMode(servoPin_Body, OUTPUT);
pinMode(servoPin_Arm, OUTPUT);
pinMode(servoPin_Hand, OUTPUT);
Serial.begin(9600);
Serial.println("Hi, I'm ready!");
delay(1000);
Serial.println("Type command down below");
delay(100);
Serial.println("Command list:");
delay(100);
Serial.println("1. AX: Moves arm up and down (x=degrees)");
delay(100);
Serial.println("2. TX: Moves torso forward and back");
delay(100);
Serial.println("3. HX: Opens and closes hands");
delay(100);
Serial.println("4. BX: Moves body to the left and to the right");
delay(100);
Serial.println("Okay, let's get started!");
}
void loop()
{
while (Serial.available() > 0) {
val = Serial.read();
String x = Serial.readString();
int mob = x.toInt();
if (!flag_recv_1st_sym) {
if ((val == 'A') || (val == 'T') || (val == 'H') || (val == 'B')) {
tmp = val - 'A';
flag_recv_1st_sym = 1;
}
else if (mob >= 0 && mob <= 180)
{
val = mob;
Serial.print("Moving ");
Serial.print(val);
Serial.println(" degrees");
// giving the servo time
//to rotate to commanded position
for (int i = 0; i <= 50; i++)
{
//servoPulse(servoPin, val);
switch (tmp)
{
case 0:
TURN_ARM;
break;
case 19:
TURN_TORSO;
break;
case 7:
TURN_HAND;
break;
case 1:
TURN_BODY;
break;
}
}
}
flag_recv_1st_sym = 0;
}
}
}
// define a servo pulse function
void servoPulse(int pin, int angle)
{
// convert angle to 500-2480 pulse width
int pulseWidth = (angle * 11) + 500;
digitalWrite(pin, HIGH); // set the level of servo pin as high
delayMicroseconds(pulseWidth); // delay microsecond of pulse width
digitalWrite(pin, LOW); // set the level of servo pin as low
delay(20 - pulseWidth / 1000);
}