Помогите с работой сервопривода и двигателей

Здрасьте! Помогите, пожалуйста, с одной проблемкой. Делаю машинку на ардуино, так сказать вникаю с простого.Исполюзую ардуино нано, l298n, два двигателя, HC-05, и серва SG90. Заливаю скетч с управлением только двигателями и все работает, заливаю скетч управлением только сервой и тоже работает, но вот скрестить их никак не получается. Серва работает, а двигатели нет. Подскажите, что не так....спасибо!

Код (C++):

#include <Servo.h>
const int TIMEOUT_TIME_MS = 150;
unsigned long lastPilotSymbolTime;
char symbol;

int val;
int IN1 = 7;
int IN2 = 6;
int IN3 = 5;
int IN4 = 4;
int LED = 13;
enum States
{
  WAITING,
  READING,
  RUNNING,
  ERROR,
  TIMEOUT
};

States state;
States onWait();
States onRead();
States onRun();
States onError();

States onTimeout();

class Servo_extend : public Servo {
public:
  void up(){
   if(this->angle >= 140){
    this->angle = 140;
   } else {
    this->angle += this->speed;
   }
   this->write(this->angle);
  }
  void down(){
   if(this->angle <= 35){
    this->angle = 35;
   } else {
    this->angle -= this->speed;
   }
   this->write(this->angle);
  }
  void rotate(int angle){
 
   if(this->speed <= 40){
    this->angle = angle;
    this->write(angle);
    return;
   }
   if(angle > this->angle){
    for(this->angle; this->angle <= angle; this->angle ++){
     this->write(this->angle);
     delay(this->speed);
    }
   } else {
    for(this->angle; this->angle >= angle; this->angle --){
     this->write(this->angle);
     delay(this->speed);
    }
   }
  }
 
  void setAngle(int angle){
   this->angle = angle;
   this->write(angle);
  }
  void setSpeed(int speed){
   this->speed = speed;
  }
private:
  int angle = 0, speed = 40;
};
Servo_extend servo0;
void setup()
{
  Serial.begin(9600);
  lastPilotSymbolTime = 0;
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

 
  pinMode(LED, OUTPUT);
  digitalWrite(LED, HIGH);


servo0.attach(9);


servo0.setAngle(90);


servo0.setSpeed(40);

Serial.begin(9600);

}
char incomingByte;
void loop()
{
 
if( Serial.available() > 0 ){
incomingByte = Serial.read();
Serial.println(incomingByte);
switch(incomingByte){
  case '1':
   servo0.up();
   break;
  case '2':
   servo0.down();
   break;
  case '0':
   servo0.setAngle(90);
   break;
  }
}
  switch (state)
  {
  case WAITING:
    state = onWait();
    break;
  case READING:
    state = onRead();
    break;
  case RUNNING:
    state = onRun();
    break;
  case TIMEOUT:
    state = onTimeout();
    break;
  default:
    state = onError();
  }
}

States onWait()
{
  if (Serial.available() > 0)
  {
    return READING;
  }
  if (lastPilotSymbolTime && (millis() - lastPilotSymbolTime > TIMEOUT_TIME_MS))
  {
    return TIMEOUT;
  }
  return WAITING;
}

States onRead()
{
  symbol = Serial.read();
  return RUNNING;
}

States onRun()
{
  switch (symbol)
  {
  case 'W':
    //начало действий при полученном символе 'W' (вперед)
   
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
    break;
  case 'S':
    //начало действий при полученном символе 'S' (назад)
   
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    break;
  default:
    return ERROR;
  }
  return WAITING;
}

States onError()
{
 
  while (Serial.available())
  {
    Serial.read();
  }
  return WAITING;
}

States onTimeout()
{
 
 
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  if (Serial.available())
  {
    return READING;
  }
  return TIMEOUT;
}

 

Спасибо, все так старались помочь. Сам сел и сделал.

 

И в чем было дело?

 

ну так молодец!
значит можешь если захочешь.