CODIGO PROPUESTA CURVA 90º

CÓDIGO HECHO POR: MOH Nº 21, MFN Nº6, IGT Nº8

/***   Included libraries  ***/


/***   Global variables and function definition  ***/
const int IR_izq= 2;
const int IR_derch = 3;
const int IN_1 = 11;
const int IN_3 = 12;
const int IN_4 = 13;
const int IN_2 = 10;
const int ENA = 5;
const int ENB = 6;

float velocidad = 100;
void AVANZAR() {
    digitalWrite(IN_1, HIGH);
    digitalWrite(IN_2, LOW);
    digitalWrite(IN_3, HIGH);
    digitalWrite(IN_4, LOW);
    analogWrite(6, velocidad);
    analogWrite(5, velocidad);
}
void DERECHA() {
    digitalWrite(IN_1, HIGH);
    digitalWrite(IN_2, LOW);
    digitalWrite(IN_3, LOW);
    digitalWrite(IN_4, HIGH);
    analogWrite(6, velocidad);
    analogWrite(5, velocidad);
}
void IZQUIERDA() {
    digitalWrite(IN_1, LOW);
    digitalWrite(IN_2, HIGH);
    digitalWrite(IN_3, HIGH);
    digitalWrite(IN_4, LOW);
    analogWrite(6, velocidad);
    analogWrite(5, velocidad);
}
void DETENER() {
    digitalWrite(ENA, LOW);
    digitalWrite(ENB, LOW);
    digitalWrite(IN_1, LOW);
    digitalWrite(IN_2, LOW);
    digitalWrite(IN_3, LOW);
    digitalWrite(IN_4, LOW);
}



/***   Setup  ***/
void setup() {
    pinMode(IR_izq, INPUT);
    pinMode(IR_derch, INPUT);
    pinMode(IN_1, OUTPUT);
    pinMode(IN_3, OUTPUT);
    pinMode(IN_4, OUTPUT);
    pinMode(IN_2, OUTPUT);
    pinMode(ENA, OUTPUT);
    pinMode(ENB, OUTPUT);

}


/***   Loop  ***/
void loop() {
    float IR_izq = digitalRead(IR_derch);

    float IR_derch = digitalRead(IR_izq);
    if (((IR_izq == 1) && (IR_derch == 1))) {
        AVANZAR();
    } else if (((IR_izq == 1) && (IR_derch == 0))) {
        DERECHA();
    } else if (((IR_izq == 0) && (IR_derch == 1))) {
        IZQUIERDA();
    } else if (((IR_izq == 0) && (IR_derch == 0))) {

        DERECHA();
        delay(2000);
        if (((IR_izq == 0) && (IR_derch == 0))) {
            IZQUIERDA();
        } else if (((IR_izq == 0) && (IR_derch == 0))) {
            DETENER();
        }
    }
}