/* 

laser.ino

Controlling two motors with an Arduino and 
Sparkfun Motor Driver 1A Dual TB6612FNG

Reference:

http://bildr.org/2012/04/tb6612fng-arduino/

Author: Mahesh Venkitachalam
Website: electronut.in

*/

// silniczek A podłączony do A01 i A02
// silniczek B podłączony do B01 i B02

int STBY = 10; //czuwanie

// silniczek A
int PWMA = 3; //sterowanie prędkością 
int AIN1 = 9; //kierunek
int AIN2 = 8; //kierunek

// silniczek B
int PWMB = 5; //sterowanie prędkością
int BIN1 = 11; //kierunek
int BIN2 = 12; //kierunek

void setup(){

  pinMode(STBY, OUTPUT);

  pinMode(PWMA, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);

  pinMode(PWMB, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);

  // inicjowanie komunikacji szeregowej
  Serial.begin(9600);
}

// pętla główna, która odczytuje dane silniczków wysyłane przez laser.py 
void loop() 
{		 
  // wysyłane dane mają następującą postać:
  // 'H' (nagłówek), s1 (prędkość 1), d1 (kierunek 1), s2 (prędkość 2), d2 (kierunek 2)
  if (Serial.available() >= 5) {
    if(Serial.read() == 'H') {
      // odczyt kolejnych 4 bajtów
      byte s1 = Serial.read();
      byte d1 = Serial.read();
      byte s2 = Serial.read();
      byte d2 = Serial.read();
      
      // zatrzymanie silniczków, jeśli prędkości są równe 0
      if(s1 == 0 && s2 == 0) {
        stop();
      }
      else {
        // ustawienie prędkości i kierunków obu silniczków
        move(0, s1, d1);
        move(1, s2, d2);
      }
      // krótka pauza 20 ms
      delay(20);
    }
    else {
      // jeśli dane są nieprawidłowe, zatrzymanie silniczków
      stop(); 
    }
  }
  else {
    // jeśli nie ma żadnych danych, pauza przez 250 ms 
    delay(250);
  }
}

// ustawianie prędkości i kierunków silniczków
// silniczek: A -> 1, B -> 0
// kierunek: 1/0 
void move(int motor, int speed, int direction)
{

  // wyjście z trybu oczekiwania
  digitalWrite(STBY, HIGH); 

  boolean inPin1 = LOW;
  boolean inPin2 = HIGH;

  if(direction == 1) {
    inPin1 = HIGH;
    inPin2 = LOW;
  }        
  
  if(motor == 1){
    digitalWrite(AIN1, inPin1);
    digitalWrite(AIN2, inPin2);
    analogWrite(PWMA, speed);
  }
  else{
    digitalWrite(BIN1, inPin1);
    digitalWrite(BIN2, inPin2);
    analogWrite(PWMB, speed);
  }
}

// zatrzymanie silniczków
void stop(){
  // włączenie trybu oczekiwania  
  digitalWrite(STBY, LOW);
}
