/*
 MagicBOT.ino
 Il programma permette il test della parte controllo motore. 
 
 Vengono utilizzati i seguenti pin
 Pin +5V     -> Alimentazione logica
 Pin GND     -> Alimentazione
 Pin Vin     -> Alimentazione motori 
 Pin 0       -> RX Modulo Bluetooth
 Pin 1       -> TX Modulo Bluetooth
 Pin 2       -> Direzione Motore A
 Pin 3       -> PWM - Velocità Motore A
 Pin 4       -> Direzione Motore B
 Pin 5       -> PWM - Velocità Motore B
 
 Ultima modifica il 5/1/2013
 da Adriano Gandolfo <https://www.adrirobot.it>
 This example code is in the public domain.
 */


int M1_PWM = 3;  //PWM controllo velocità per motore 1 - pin digitale 3 
int M2_PWM = 5;  //PWM controllo velocità per motore 2 - pin digitale 3 
int M1 = 2;      //Controllo direzione per motore 1 - pin digitale 2
int M2 = 4;      //Controllo direzione per motore 2 - pin digitale 4


void setup()
{
  Serial.begin(115200);       //imposta la porta di comunicazione con il modulo  
  pinMode(M1_PWM, OUTPUT);  //Imposta i pin come uscite
  pinMode(M2_PWM, OUTPUT);
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);

 // ferma per default entambi i motori
   digitalWrite(M1_PWM, LOW); //Disabilita il comando del motore
   digitalWrite(M2_PWM, LOW);
  delay(500);

 }
void loop(void)
{

  while (Serial.available() < 1) {
  }                         // Attesa sino a quando riceve un carattere
  char val = Serial.read(); // Legge il carattere dal modulo Bluetooth e lo salva nella variabile val
  switch(val)               // Eseguie i comandi in base al carattere ricevuto
  {
  case '8':// Se il carattere ricevuto è '8' esegue la routine Robot avanti
    robot_avanti ();
    break;
  case '2'://Se il carattere ricevuto è '2' esegue la routine Robot indietro
    robot_indietro ();
    break;
  case '4'://Se il carattere ricevuto è '4' esegue la routine Robot sinistra
    robot_sinistra ();
    break;
  case '6'://Se il carattere ricevuto è '6' esegue la routine Robot destra
    robot_destra ();
    break;
  case '0'://Se il carattere ricevuto è '0' esegue la routine Robot fermo
    stop();
    break;
  default:
    stop(); // Ferma il robot
    break;
}
}
void stop(void) //Robot fermo
{
   digitalWrite(M1_PWM, LOW); //Disabilita il comando del motore
   digitalWrite(M2_PWM, LOW);
}
void robot_indietro()
{
  digitalWrite(M1, LOW);
  analogWrite(M1_PWM, 255);
  digitalWrite(M2, HIGH);
  analogWrite(M2_PWM, 255);
}
void robot_avanti ()
{
  digitalWrite(M1, HIGH);
  analogWrite(M1_PWM, 255);
  digitalWrite(M2, LOW);
  analogWrite(M2_PWM, 255);
}
void robot_destra ()
{
  digitalWrite(M1, LOW);
  analogWrite(M1_PWM, 255);
  digitalWrite(M2, LOW);
  analogWrite(M2_PWM, 255); 
 }
void robot_sinistra ()
{
  digitalWrite(M1, HIGH);
  analogWrite(M1_PWM, 255);
  digitalWrite(M2, HIGH);
  analogWrite(M2_PWM, 255); 
 }