/*

Controlador del motor TB6612FNG

Motor Driver Arduino
Sam Bhattacharyya
*/
// Asinar los pins conectados a los portes diferentes del TB6612FNG
const int PUSH_D =3;
const int PUSH_F = 12;
const int PUSH_I = 2 ;
const int IR = A0;
const int STBY = 8;
const int AIN1 = 7;
const int AIN2 = 6;
const int BIN1 = 9;
const int BIN2 = 10;
const int PWMA = 5;
const int PWMB = 11;
int edo_1 = 0;
int SA;
int PUSH_D_r = 0;
int PUSH_F_r = 0;
int PUSH_I_r = 0;
int InfraRed=0.0;
void avanzar()
{
digitalWrite(STBY, HIGH); // Deber ser HIGH para habilitar el driver
digitalWrite(AIN1, HIGH); // Logica para que mueva adelante el motor A
digitalWrite(AIN2, LOW);
digitalWrite(BIN1, HIGH); // Logica para que mueva adelante el motor B
digitalWrite(BIN2, LOW);
analogWrite(PWMA, 75); // Velocidad motor A (0 - 255);
analogWrite(PWMB, 75); // Velocidad motor B (0-255);
}
void retroceder()
{
digitalWrite(STBY, HIGH); // Deber ser HIGH para habilitar el driver
digitalWrite(AIN1, LOW); // Logica para que mueva adelante el motor A
digitalWrite(AIN2, HIGH);
digitalWrite(BIN1, LOW); // Logica para que mueva adelante el motor B
digitalWrite(BIN2, HIGH);
analogWrite(PWMA, 50); // Velocidad motor A (0 - 255);
analogWrite(PWMB, 50); // Velocidad motor B (0-255);

}
void izquierda()
{
digitalWrite(STBY, HIGH); // Deber ser HIGH para habilitar el driver
digitalWrite(AIN1, LOW); // Logica para que mueva adelante el motor A
digitalWrite(AIN2, HIGH);
digitalWrite(BIN1, HIGH); // Logica para que mueva adelante el motor B
digitalWrite(BIN2, LOW);
analogWrite(PWMA, 50); // Velocidad motor A (0 - 255);
analogWrite(PWMB, 50); // Velocidad motor B (0-255);

}
void derecha()
{
digitalWrite(STBY, HIGH); // Deber ser HIGH para habilitar el driver
digitalWrite(AIN1, HIGH); // Logica para que mueva adelante el motor A
digitalWrite(AIN2, LOW);
digitalWrite(BIN1, LOW); // Logica para que mueva adelante el motor B
digitalWrite(BIN2, HIGH);
analogWrite(PWMA, 50); // Velocidad motor A (0 - 255);
analogWrite(PWMB, 50); // Velocidad motor B (0-255);
}
void stop_()
{
digitalWrite(STBY, HIGH); // Deber ser HIGH para habilitar el driver
digitalWrite(AIN1, LOW); // Logica para que mueva adelante el motor A
digitalWrite(AIN2, LOW);
digitalWrite(BIN1, LOW); // Logica para que mueva adelante el motor B
digitalWrite(BIN2, LOW);
analogWrite(PWMA, 0); // Velocidad motor A (0 - 255);
analogWrite(PWMB, 0); // Velocidad motor B (0-255);
}
void setup() {
Serial.begin(115200);
// Los siguientes son de output simple digital

pinMode(PUSH_D, INPUT); // Boton derecha
pinMode(PUSH_F, INPUT); // Boton adelante
pinMode(PUSH_I, INPUT); // Boton izquierda
pinMode(STBY, OUTPUT);
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(IR, INPUT); // Sensor infrarrojo
}
void loop()
{
char c = 0;
int p1=0,p2=0,p3=0,p4=0;
if(Serial.available()); // Si hay datos disponibles en el buffer
c = Serial.read();

switch (c) {
case 'A':
// Iniciar busqueda del objetivo
Serial.println(InfraRed);
derecha();

break;

case 'P':
// Terminar busqueda. Alguien mas ya la encotro
stop_();
break;

} // fin del switch

char envio = '1';
InfraRed=digitalRead(IR); // Lee la entrada del Sensor que detectara al objet
ivo.
if (InfraRed == 0){
Serial.println('1');
}

}
// Funcion que realiza el proceso de busqueda del objetivo programado como secue
ncia.
void Iniciar_busqueda()
{

PUSH_D_r = digitalRead(PUSH_D); // Lee la salida del boton derecho.
PUSH_F_r = digitalRead(PUSH_F); // Lee la salida del boton del frente.
PUSH_I_r = digitalRead(PUSH_I); // Lee la salida del boton izquierdo.
//Serial.println(InfraRed); // Imprime la e
//delay(100);


// Si boton derecho es presionado
if (PUSH_D_r == 1)
{
stop_();
izquierda();
delay(100);

}
// Si boton del frente es presionado
else if (PUSH_F_r == 1)
{
stop_();
delay(100);
}

// Si boton de la izquierda es presionado
else if (PUSH_I_r == 1)
{
stop_();
derecha();
delay(100);
}

else {
avanzar();
delay(100);
}
}