#include <Servo.h>


Servo ServoMot_1;
Servo ServoMot_2;
//int left=90;
//int right=90;
int posLR=90;
int posUD=110;
//int up=110;
//int down=110;

void setup()
{

  
ServoMot_1.attach(5);
ServoMot_2.attach(6);
ServoMot_1.write(90);
ServoMot_2.write(110);

//Setup Canale  A
pinMode(12, OUTPUT); //Inizializza  Canale A pin
pinMode(9, OUTPUT); //Inizializza Fermo Canale A pin
pinMode(13, OUTPUT); //Inizializza  Canale B pin
pinMode(8, OUTPUT); //Inizializza Fermo Canale B pin

Serial.begin(9600);



}


void loop()
{
  

 if (Serial.available()) {
    int inByte = Serial.read();
 
     // UP
    if (inByte==117 && posUD <= 179) 
    {         
      posUD = posUD + 1 ;
      ServoMot_2.write(posUD);  
    }
    // DOWN
    if (inByte==100 && posUD >= 80) 
    {     
      posUD = posUD - 1 ;
      ServoMot_2.write(posUD);
    } 
     //Left
    if (inByte==108 && posLR >= 1) {
      posLR = posLR - 1 ;
      ServoMot_1.write(posLR);
    
    }
    //Right
    if (inByte==114 && posLR <= 179) 
    {
      posLR = posLR + 1 ;
      ServoMot_1.write(posLR);
    
    }
    
 

 // Controllo Motori

 
     // AVANTI
    if (inByte==97) 
    {         
      digitalWrite(12, LOW);  // AVANTI Motore A
      digitalWrite(13, HIGH); // AVANTI Motore B
      digitalWrite(9, LOW);   //Sblocco  A
      digitalWrite(8, LOW);   //Sblocco B
      analogWrite(3, 255);   // Velocità Motore A
      analogWrite(11, 255);   // Velocità Motore B
      delay(10);
      digitalWrite(9, HIGH); //Blocco Motore A
      digitalWrite(8, HIGH); //Blocco Motore B
    }
    // INDIETRO
    if (inByte==105 ) 
    {     
      digitalWrite(12, HIGH);  // INDIETRO Motore A
      digitalWrite(13, LOW); // INDIETRO Motore B
      digitalWrite(9, LOW);   //Sblocco  A
      digitalWrite(8, LOW);   //Sblocco B
      analogWrite(3, 255);   // Velocità Motore A
      analogWrite(11, 255);   // Velocità Motore B
      delay(10);
      digitalWrite(9, HIGH); //Blocco Motore A
      digitalWrite(8, HIGH); //Blocco Motore B
    } 
     //Left 's' = 115 (ascii)
    if (inByte==115) {
      digitalWrite(12, LOW);  // INDIETRO Motore A
      digitalWrite(13, LOW); // INDIETRO Motore B
      digitalWrite(9, LOW);   //Sblocco  A
      digitalWrite(8, LOW);   //Sblocco B
      analogWrite(3, 255);   // Velocità Motore A
      analogWrite(11, 255);   // Velocità Motore B
      delay(10);
      digitalWrite(9, HIGH); //Blocco Motore A
      digitalWrite(8, HIGH); //Blocco Motore B    

    
    }
    //Right 'e' = 101 (ascii)
    if (inByte==101) 
    {
      digitalWrite(12, HIGH);  // INDIETRO Motore A
      digitalWrite(13, HIGH); // INDIETRO Motore B
      digitalWrite(9, LOW);   //Sblocco  A
      digitalWrite(8, LOW);   //Sblocco B
      analogWrite(3, 255);   // Velocità Motore A
      analogWrite(11, 255);   // Velocità Motore B
      delay(10);
      digitalWrite(9, HIGH); //Blocco Motore A
      digitalWrite(8, HIGH); //Blocco Motore B    
    
    }
    
  }
 
 
}