Ошибок нет, но код не работает

Помогите, пожалуйста! При наборе в терминал команд например 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);

}

Ответы (0 шт):