
/*
 * Prueba de robot R2D2
 * (c) 2016 Copyright Leonardo Yamasaki Maza
 * Licencia GPL.
 */

// General defines
#define MOTOR1P    5 //Motor 1 positivo
#define MOTOR1N    4 //Motor 1 negativo
#define MOTOR2P    3 //Motor 2 positivo
#define MOTOR2N    2 //Motor 2 negativo
#define SERVO      6 //Señal del servo cable amarilo
#define DELAYHEAD 15 //Retardo de posición del servo
#define RXBLUE    10 //Pin conectar a TX del modulo Bluetooth
#define TXBLUE    11 //Pin conectar a RX del modulo Bluetooth

//Settings para servo mueve cabeza
#include 
Servo head;
int pos = 0;

//Variables de control
int espera;
char cmd;

//Comunicacion con bluetooth
#include 
SoftwareSerial softSerial(RXBLUE, TXBLUE); // RX, TX

// Movimiento del servo para cabeza
void headmove(char c) {
  if(c=='d') {
    for (pos = 90; pos >= 0; pos -= 1) {
      head.write(pos);
      delay(DELAYHEAD);
    }
    for (pos = 0; pos <= 90; pos += 1) {
      head.write(pos);
      delay(DELAYHEAD);
    }
  }
  if(c=='i') {
    for (pos = 90; pos <= 180; pos += 1) {
      head.write(pos);
      delay(DELAYHEAD);
    }
    for (pos = 180; pos >= 90; pos -= 1) {
      head.write(pos);
      delay(DELAYHEAD);
    }
  }
  if(c=='c') {
    pos = 90;
    head.write(pos);
    delay(DELAYHEAD);
  }
  if(c=='m') {
    for (pos = 90; pos >= 0; pos -= 1) {
      head.write(pos);
      delay(DELAYHEAD);
    }
    for (pos = 0; pos <= 90; pos += 1) {
      head.write(pos);
      delay(DELAYHEAD);
    }
        for (pos = 90; pos <= 180; pos += 1) {
      head.write(pos);
      delay(DELAYHEAD);
    }
    for (pos = 180; pos >= 90; pos -= 1) {
      head.write(pos);
      delay(DELAYHEAD);
    }
  }

}

/*
 * Control de motores
*/

void parar(){
  digitalWrite(MOTOR1P,LOW);
  digitalWrite(MOTOR1N,LOW);
  digitalWrite(MOTOR2P,LOW);
  digitalWrite(MOTOR2N,LOW);
}

void adelante(){
  digitalWrite(MOTOR1P,HIGH);
  digitalWrite(MOTOR1N,LOW);
  digitalWrite(MOTOR2P,HIGH);
  digitalWrite(MOTOR2N,LOW);
}

void atras(){
  digitalWrite(MOTOR1P,LOW);
  digitalWrite(MOTOR1N,HIGH);
  digitalWrite(MOTOR2P,LOW);
  digitalWrite(MOTOR2N,HIGH);
}

void derecha(){
  digitalWrite(MOTOR1P,HIGH);
  digitalWrite(MOTOR1N,LOW);
  digitalWrite(MOTOR2P,LOW);
  digitalWrite(MOTOR2N,HIGH);
}

void izquierda(){
  digitalWrite(MOTOR1P,LOW);
  digitalWrite(MOTOR1N,HIGH);
  digitalWrite(MOTOR2P,HIGH);
  digitalWrite(MOTOR2N,LOW);
}

/*
 * Setup y loop
 * 
 */
void setup() {
  pinMode(MOTOR1P,OUTPUT);
  pinMode(MOTOR1N,OUTPUT);
  pinMode(MOTOR2P,OUTPUT);
  pinMode(MOTOR2N,OUTPUT);
  head.attach(SERVO);
  headmove('c');
  // set the data rate for the SoftwareSerial port
  softSerial.begin(9600);
  softSerial.println("Hello, R2D2!");
  // Open serial communications and wait for port to open:
  Serial.begin(9600);
  while (!Serial) {
    ;
  }
  Serial.println("En espera...\r\n");
}

void loop() {
  if (softSerial.available()) {
    cmd = softSerial.read();
    Serial.write(cmd);
    Serial.write(':');
  }
  switch(cmd){
    case 'f':
      adelante();
      Serial.println("Adelante");
      espera=1;
      break;
    case 'b':
      atras();
      Serial.println("Atras");
      espera=1;
      break;
    case 'r':
      derecha();
      Serial.println("Derecha");
      espera=1;
      break;
    case 'l':
      izquierda();
      Serial.println("Izquierda");
      espera=1;
      break;
    case 's':
      parar();
      headmove('c');
      Serial.println("Parar");
      espera=1;
      break;
    case 'i':
      Serial.println("Cabeza izquierda");
      espera=1;
      headmove('i');
      break;
    case 'd':
      Serial.println("Cabeza derecha");
      espera=1;
      headmove('d');
      break;
    case 'm':
      Serial.println("Cabeza escaneando...");
      espera=1;
      headmove('m');
      break;
    default:
      if(espera) {
        Serial.println("En espera...\r\n");
        espera=0;
      }
      break;
  } //End switch
  cmd=0;
}
//END