#include <Servo.h>

Servo myservo1;  // create servo object to control a servo
char unChar;
String readString;


#define pin1 2 // motor #1 +
#define pin2 3 // motor #1 –
#define pw1 9 // motor #1 pwm
#define pin3 4 // motor #2 +
#define pin4 5 // motor #2 –
#define pw2 6 // motor #2 pwm




void setup() {
  pinMode(pin1, OUTPUT);
  pinMode(pin2, OUTPUT);
  pinMode(pin3, OUTPUT);
  pinMode(pin4, OUTPUT);
  pinMode(pw1, OUTPUT);
  pinMode(pw2, OUTPUT);

  myservo1.attach(10);  //the pin for the servo1 control


  Serial.begin(9600);
  digitalWrite(pin1, LOW);
  digitalWrite(pin2, LOW);
  digitalWrite(pin3, LOW);
  digitalWrite(pin4, LOW);
  analogWrite(pw1, 10);
  analogWrite(pw2, 10);



}

void loop() {
  if (Serial.available()) {  //verify firs character in serial port
    unChar = Serial.read();

    if (unChar == 'A') { //if A go to motor1 function
      motor1();
    }

    if (Serial.available() >= 2 )
    {
      unsigned int a = Serial.read();
      unsigned int b = Serial.read();
      unsigned int val = (b * 256) + a;

      if (val == 100) // motor 1 reverse
      {
        digitalWrite(pin1, LOW);
        digitalWrite(pin2, HIGH);

      }
      else if (val == 200) // motor #1 stop
      {
        digitalWrite(pin1, LOW);
        digitalWrite(pin2, LOW);
      }
      else if (val == 300) // motor #1 forward
      {
        digitalWrite(pin1, HIGH);
        digitalWrite(pin2, LOW);
      }
      else if (val == 400) // motor #2 reverse
      {
        digitalWrite(pin3, LOW);
        digitalWrite(pin4, HIGH);
      }
      else if (val == 500) // motor #2 stop
      {
        digitalWrite(pin3, LOW);
        digitalWrite(pin4, LOW);
      }
      else if (val == 600) // motor #2 forward
      {
        digitalWrite(pin3, HIGH);
        digitalWrite(pin4, LOW);
      }
      else if (val >= 1000 && val <= 1255)
      {
        analogWrite (pw1, val - 1000);
      } //}
      else if (val >= 2000 && val <= 2255)
      {
        analogWrite (pw2, val - 2000);
      }
    }
  }
}
void motor1() {
  delay(10);
  while (Serial.available()) { //Now the numerical data of the angle of the servomotor is received
    //delayMicroseconds(100);
    char c = Serial.read();  // Se leen los caracteres que ingresan por el puerto
    readString += c;         //each character builds in a string
  }
  if (readString.length() > 0) {  //the length of the string is verified
    Serial.println(readString.toInt());  //Now we send data to serial and servos
    myservo1.write(readString.toInt());
    readString = ""; // Clear string
  }
}