const int pinDI = 2; // Detectores

const int pinDF = 3;

const int pinDD = 4;

const int pinLI = 5; // Indicadores LED

const int pinLF = 6;
const int pinLD = 7;

const int pinAIN1 = 8; // Control direcci�n Motor Izquierdo

const int pinAIN2 = 9;
const int pinPWMA = 10; // Control velocidad Motor Izquierdo
const int pinBIN1 = 14; // Control direcci�n Motor Derecho
const int pinBIN2 = 15;
const int pinPWMB = 11; // Control velocidad Motor Derecho

const int pinServo = 12; // Control Servomotor

int DI = LOW; // Variables para almacenar el estado de los detectores

int DD = LOW;
int DF = LOW;

int velMotor = 150; // 150 ~= 50%. Rango de velocidad: [0-255]

void setup()
{
pinMode(pinDI, INPUT_PULLUP);
pinMode(pinDF, INPUT_PULLUP);
pinMode(pinDD, INPUT_PULLUP);

pinMode(pinLI, OUTPUT);
pinMode(pinLF, OUTPUT);
pinMode(pinLD, OUTPUT);

pinMode(pinAIN1, OUTPUT);
pinMode(pinAIN2, OUTPUT);
pinMode(pinPWMA, OUTPUT);

pinMode(pinBIN1, OUTPUT);
pinMode(pinBIN2, OUTPUT);
pinMode(pinPWMB, OUTPUT);

pinMode(pinServo, OUTPUT);

Serial.begin( 9600 );
}

void loop()
{
// Lectura de SENSORES:
DI = digitalRead( pinDI );
DF = digitalRead( pinDF );
DD = digitalRead( pinDD );

if ((DI==LOW) && (DF==LOW) && (DD==LOW))

{
// Motor Izquierda
digitalWrite(pinAIN1, HIGH); // CW: Horario
digitalWrite(pinAIN2, LOW);
digitalWrite(pinPWMA, velMotor);
 // Motor Derecha
digitalWrite(pinBIN1, HIGH); // CW: Horario
digitalWrite(pinBIN2, LOW);
digitalWrite(pinPWMB, velMotor);
// Servomotor
digitalWrite(pinServo, 0); // 0 -> ON , 180 -> OFF
// Activamos o desactivamos LED:
digitalWrite(pinLI, LOW);
digitalWrite(pinLF, LOW);
digitalWrite(pinLD, LOW);
// Mensaje (Monitor Serial - Arduino):
Serial.println("Robot: Avanzando y Limpiando");
}
else if ((DI==LOW) && (DF==LOW) && (DD==HIGH))
{
// Motor Izquierda
digitalWrite(pinAIN1, LOW); // OFF
digitalWrite(pinAIN2, LOW);
digitalWrite(pinPWMA, 0);
// Motor Derecha
digitalWrite(pinBIN1, HIGH); // CW: Horario
digitalWrite(pinBIN2, LOW);
digitalWrite(pinPWMB, velMotor);
// Servomotor
digitalWrite(pinServo, 0); // 0 -> ON , 180 -> OFF
// Activamos o desactivamos LED:
digitalWrite(pinLI, LOW);
digitalWrite(pinLF, LOW);
digitalWrite(pinLD, HIGH);
// Mensaje (Monitor Serial - Arduino):
Serial.println("Robot: Girando a Izquierda y Limpiando");
}
else if ((DI==LOW) && (DF==HIGH) && (DD==LOW))
{
// Motor Izquierda
digitalWrite(pinAIN1, LOW); // CCW: Antihorario
digitalWrite(pinAIN2, HIGH);
digitalWrite(pinPWMA, velMotor);
// Motor Derecha
digitalWrite(pinBIN1, LOW); // CCW: Antihorario
digitalWrite(pinBIN2, HIGH);
digitalWrite(pinPWMB, velMotor);
// Servomotor
digitalWrite(pinServo, 180); // 0 -> ON , 180 -> OFF
// Activamos o desactivamos LED:
digitalWrite(pinLI, LOW);
digitalWrite(pinLF, HIGH);
digitalWrite(pinLD, LOW);
// Mensaje (Monitor Serial - Arduino):
Serial.println("Robot: Retrocediendo y NO Limpiando");
}
else if ((DI==LOW) && (DF==HIGH) && (DD==HIGH))
{
// Motor Izquierda
digitalWrite(pinAIN1, LOW); // OFF
digitalWrite(pinAIN2, LOW);
digitalWrite(pinPWMA, 0);
// Motor Derecha
digitalWrite(pinBIN1, HIGH); // CW: Horario
 digitalWrite(pinBIN2, LOW);
digitalWrite(pinPWMB, velMotor);
// Servomotor
digitalWrite(pinServo, 0); // 0 -> ON , 180 -> OFF
// Activamos o desactivamos LED:
digitalWrite(pinLI, LOW);
digitalWrite(pinLF, HIGH);
digitalWrite(pinLD, HIGH);
// Mensaje (Monitor Serial - Arduino):
Serial.println("Robot: Girando a Izquierda y Limpiando");
}
else if ((DI==HIGH) && (DF==LOW) && (DD==LOW))
{
// Motor Izquierda
digitalWrite(pinAIN1, HIGH); // CW: Horario
digitalWrite(pinAIN2, LOW);
digitalWrite(pinPWMA, velMotor);
// Motor Derecha
digitalWrite(pinBIN1, LOW); // OFF
digitalWrite(pinBIN2, LOW);
digitalWrite(pinPWMB, 0);
// Servomotor
digitalWrite(pinServo, 0); // 0 -> ON , 180 -> OFF
// Activamos o desactivamos LED:
digitalWrite(pinLI, HIGH);
digitalWrite(pinLF, LOW);
digitalWrite(pinLD, LOW);
// Mensaje (Monitor Serial - Arduino):
Serial.println("Robot: Girando a Derecha y Limpiando");
}
else if ((DI==HIGH) && (DF==LOW) && (DD==HIGH))
{
// Motor Izquierda
digitalWrite(pinAIN1, HIGH); // CW: Horario
digitalWrite(pinAIN2, LOW);
digitalWrite(pinPWMA, velMotor);
// Motor Derecha
digitalWrite(pinBIN1, HIGH); // CW: Horario
digitalWrite(pinBIN2, LOW);
digitalWrite(pinPWMB, velMotor);
// Servomotor
digitalWrite(pinServo, 0); // 0 -> ON , 180 -> OFF
// Activamos o desactivamos LED:
digitalWrite(pinLI, HIGH);
digitalWrite(pinLF, LOW);
digitalWrite(pinLD, HIGH);
// Mensaje (Monitor Serial - Arduino):
Serial.println("Robot: ");
}
else if ((DI==HIGH) && (DF==HIGH) && (DD==LOW))
{
// Motor Izquierda
digitalWrite(pinAIN1, HIGH); // CW: Horario
digitalWrite(pinAIN2, LOW);
digitalWrite(pinPWMA, velMotor);
// Motor Derecha
digitalWrite(pinBIN1, LOW); // OFF
digitalWrite(pinBIN2, LOW);
digitalWrite(pinPWMB, 0);
 // Servomotor
digitalWrite(pinServo, 0); // 0 -> ON , 180 -> OFF
// Activamos o desactivamos LED:
digitalWrite(pinLI, HIGH);
digitalWrite(pinLF, HIGH);
digitalWrite(pinLD, LOW);
// Mensaje (Monitor Serial - Arduino):
Serial.println("Robot: Robot: Girando a Derecha y Limpiando");
}
else if ((DI==HIGH) && (DF==HIGH) && (DD==HIGH))
{
// Motor Izquierda
digitalWrite(pinAIN1, LOW); // CCW: Antihorario
digitalWrite(pinAIN2, HIGH);
digitalWrite(pinPWMA, velMotor);
// Motor Derecha
digitalWrite(pinBIN1, LOW); // CCW: Antihorario
digitalWrite(pinBIN2, HIGH);
digitalWrite(pinPWMB, velMotor);
// Servomotor
digitalWrite(pinServo, 180); // 0 -> ON , 180 -> OFF
// Activamos o desactivamos LED:
digitalWrite(pinLI, HIGH);
digitalWrite(pinLF, HIGH);
digitalWrite(pinLD, HIGH);
// Mensaje (Monitor Serial - Arduino):
Serial.println("Robot: Retrocediendo y NO Limpiando");
}
}