// Import the Arduino Servo library
#include <Servo.h>

struct MOTOR    // структура для хранения номеров pin-ов, к которым подключены моторчики
{
  int dir;      // DIRECTION
  int enable;   // ENABLE
};

// определяем порты, к которым подключены моторчики
MOTOR MOTOR1 = { 12, 11 };
MOTOR MOTOR2 = { 8, 10 };

// Create a Servo object for each servo
Servo servo1;
Servo servo2;
// TO ADD SERVOS:
//   Servo servoN;
//   etc...

// Common servo setup values
int minPulse = 600;   // minimum servo position, us (microseconds)
int maxPulse = 2400;  // maximum servo position, us

// User input for servo and position
int userInput[3];    // raw input from serial buffer, 3 bytes
int startbyte;       // start byte, begin reading input
int servo;           // which servo to pulse?
int pos;             // servo angle 0-180
int i;               // iterator

void setup()
{
  // Attach each Servo object to a digital pin
  servo1.attach(2, minPulse, maxPulse);
  servo2.attach(3, minPulse, maxPulse);
  // TO ADD SERVOS:
  //   servoN.attach(YOUR_PIN, minPulse, maxPulse);
  //   etc...
  pinMode(MOTOR1.dir, OUTPUT); // настраиваем выводы на вывод
  pinMode(MOTOR2.dir, OUTPUT);
  

  // Open the serial connection, 9600 baud
  Serial.begin(9600);
}

void forward1(int pos) // первый вперёд
{
  digitalWrite(MOTOR1.dir, HIGH);
  analogWrite(MOTOR1.enable, pos);
}

void forward2(int pos) // второй вперёд
{
  digitalWrite(MOTOR2.dir, HIGH);
  analogWrite(MOTOR2.enable, pos);
}

void loop()
{
  // Wait for serial input (min 3 bytes in buffer)
  if (Serial.available() > 2) {
    // Read the first byte
    startbyte = Serial.read();
    // If it's really the startbyte (255) ...
    if (startbyte == 255) {
      // ... then get the next two bytes
      for (i=0;i<2;i++) {
        userInput[i] = Serial.read();
      }
      // First byte = servo to move?
      servo = userInput[0];
      // Second byte = which position?
      pos = userInput[1];
      // Packet error checking and recovery
      if (pos == 255) { servo = 255; }

      // Assign new position to appropriate servo or motor
      switch (servo) {
        case 1:
          servo1.write(pos);    // move servo1 to 'pos'
          break;
        case 2:
          servo2.write(pos);
          break;
        case 3:
          forward1(pos);
          break;
        case 4:
          forward2(pos);
          break;
   // TO ADD SERVOS:
   //     case 5:
   //       servo5.write(pos);
   //       break;
   // etc...
      }
    }
  }


}