// CANDY-HO Argentina www.candy-ho.com
int motorIP = 4;
int motorIN = 6;
int motorDP = 3;
int motorDN = 5;
int enableA = A1;
int enableB = A0;
int SL;
int SR;
int SM;
#define LT_R !digitalRead(2)
#define LT_M !digitalRead(8)
#define LT_L !digitalRead(9)


void setup() {
  pinMode(motorIP, OUTPUT);
  pinMode(motorIN, OUTPUT);
  pinMode(motorDP, OUTPUT);
  pinMode(motorDN, OUTPUT);
  pinMode(LT_R, INPUT);
  pinMode(LT_M, INPUT);
  pinMode(LT_L, INPUT);
  Serial.begin(9600);
}

void loop()
{
  SL = digitalRead(LT_L);
  SR = digitalRead(LT_R);
  SM = digitalRead(LT_M);
  Serial.println("--");
  Serial.println(SL);
  Serial.println(SM);
  Serial.println(SR);
 // delay(1599);
  if (SM == HIGH)
   {
    forward();
       }
   else
    {
    if (SL == HIGH & SR == LOW)
    {
    left();
    delay(10);
    }
    else
    {
    if (SR == HIGH & SL == LOW) 
    {
    right();
    delay(10);
    }
    else 
        {
    backward();
    }
        }
   }
}



//Defining functions so it's more easy
//motor functions
void motorAforward() {
  digitalWrite (motorIP, HIGH);
  digitalWrite (motorIN, LOW);
}
void motorBforward() {
  digitalWrite (motorDP, HIGH);
  digitalWrite (motorDN, LOW);
}
void motorAbackward() {
  digitalWrite (motorIP, LOW);
  digitalWrite (motorIN, HIGH);
}
void motorBbackward() {
  digitalWrite (motorDP, LOW);
  digitalWrite (motorDN, HIGH);
}
void motorAstop() {
  digitalWrite (motorIP, HIGH);
  digitalWrite (motorIN, HIGH);
}
void motorBstop() {
  digitalWrite (motorDP, HIGH);
  digitalWrite (motorDN, HIGH);
}
void motorAcoast() {
  digitalWrite (motorIP, LOW);
  digitalWrite (motorIN, LOW);
}
void motorBcoast() {
  digitalWrite (motorDP, LOW);
  digitalWrite (motorDN, LOW);
}
void motorAon() {
  digitalWrite (enableA, HIGH);
}
void motorBon() {
  digitalWrite (enableB, HIGH);
}
void motorAoff() {
  digitalWrite (enableA, LOW);
}
void motorBoff() {
  digitalWrite (enableB, LOW);
}
// Movement functions

void forward()
{
  motorAforward();
  motorBforward();
  //delay(DELAY);
  Serial.println("forward");
  //Serial.println("Moving forward");
}


void backward()
{

  //Serial.println("bk");
  Serial.println("Moving back");
  motorAbackward();
  motorBbackward();
  //delay(DELAY);
}


void left()
{
  Serial.println("lf");
  //Serial.println("Turning LEFT");
  motorAbackward();
  motorBforward();

}


void right()
{
  Serial.println("rg");
  //Serial.println("Turning Right");
  motorAforward();
  motorBbackward();
}

void stop()
{
  Serial.println("stop");
  //Serial.println("Stopping");
  digitalWrite(motorIP, LOW);
  digitalWrite(motorIN, LOW);
  digitalWrite(motorDP, LOW);
  digitalWrite(motorDN, LOW);
}


void coast (int duration) {
  motorAcoast();
  motorBcoast();
  delay (duration);
}
void breakRobot (int duration) {
  motorAstop();
  motorBstop();
  delay (duration);
}
void disableMotors() {
  motorAoff();
  motorBoff();
}
void enableMotors() {
  motorAon();
  motorBon();
}


