#include<SoftwareSerial.h>
#include <Servo.h>

int motorL1 = 5;  // INPUT 1 - LEFT
int motorL2 = 6;  // INPUT 2 - LEFT
int motorR1 = 10; // INPUT 3 - RIGHT 
int motorR2 = 9;  // INPUT 4 - RIGHT

//Helpers
int vel = 500;
char dataFromApp = 'S';

void setup()
{ 
 Serial.begin(9600);
  //Output variables
  pinMode(motorL1, OUTPUT);  
  pinMode(motorL2, OUTPUT);
  pinMode(motorR1, OUTPUT);
  pinMode(motorR2, OUTPUT);  

}
 
void loop()
{
    //Wait new data
    if(Serial.available() > 0){
      dataFromApp = Serial.read();
    }
         switch (dataFromApp) {
    case 'U': //ARRIBA
      up();        
      break;
    case 'D': //ABAJO
      down();
      break;
    case 'L': //IZQUIERDA
      left();
      break;
    case 'R': //DERECHA
      right(); 
      break;
    case 'S': //ALTO 
      stop();
      break;
    }
    delay(20);
    
  
}

void up(){ //ADELANTE
  digitalWrite(motorL1, vel);
 digitalWrite(motorL2, 0);
  digitalWrite(motorR1, vel);
  digitalWrite(motorR2, 0);    
}

void down(){
 digitalWrite(motorL1, 0);
  digitalWrite(motorL2, vel);
  digitalWrite(motorR1, 0);
  digitalWrite(motorR2, vel);
}

void right(){ //derecha
  digitalWrite(motorL1, vel);
  digitalWrite(motorL2, 0);
  digitalWrite(motorR1, 0);
  digitalWrite(motorR2, 0);
 
}

void left(){
  digitalWrite(motorL1, 0);
 digitalWrite(motorL2, 0);
  digitalWrite(motorR1, vel);
  digitalWrite(motorR2, 0);
  
}

void stop(){
  digitalWrite(motorR1, 0);
  digitalWrite(motorR2, 0);
  digitalWrite(motorL1, 0);
  digitalWrite(motorL2, 0); 
  //digitalWrite

}