CÓDIGO BÁSICO (seguir línea)

GRUPO: LLC15, CGG13, RGO7


/***   Included libraries  ***/


/***   Global variables and function definition  ***/
const int IN1_11 = 11;
const int IN2_10 = 10;
const int IN3_12 = 12;
const int IN4 = 13;
const int ENB = 6;
const int ENA = 5;
const int ir_izda = 2;
const int ir_dcha = 3;

float Sensor_izda = 0;
float sensor_dcha = 0;
float VELOCIDAD = 150;
void avanza() {
    digitalWrite(IN1_11, HIGH);
    digitalWrite(IN2_10, LOW);
    digitalWrite(IN3_12, HIGH);
    digitalWrite(IN4, LOW);
    analogWrite(5, VELOCIDAD);
    analogWrite(6, VELOCIDAD);
}
void retrocede() {
    digitalWrite(IN1_11, LOW);
    digitalWrite(IN2_10, HIGH);
    digitalWrite(IN3_12, LOW);
    digitalWrite(IN4, HIGH);
    analogWrite(5, VELOCIDAD);
    analogWrite(6, VELOCIDAD);
}
void izquierda() {
    digitalWrite(IN1_11, LOW);
    digitalWrite(IN2_10, HIGH);
    digitalWrite(IN3_12, HIGH);
    digitalWrite(IN4, LOW);
    analogWrite(5, VELOCIDAD);
    analogWrite(6, VELOCIDAD);
}
void derecha() {
    digitalWrite(IN1_11, HIGH);
    digitalWrite(IN2_10, LOW);
    digitalWrite(IN3_12, LOW);
    digitalWrite(IN4, HIGH);
    analogWrite(6, VELOCIDAD);
    analogWrite(5, VELOCIDAD);
}
void parar() {
    digitalWrite(IN1_11, LOW);
    digitalWrite(IN2_10, LOW);
    digitalWrite(IN3_12, LOW);
    digitalWrite(IN4, LOW);
    digitalWrite(ENB, LOW);
    digitalWrite(ENA, LOW);
}



/***   Setup  ***/
void setup() {
    pinMode(IN1_11, OUTPUT);
    pinMode(IN2_10, OUTPUT);
    pinMode(IN3_12, OUTPUT);
    pinMode(IN4, OUTPUT);
    pinMode(ENB, OUTPUT);
    pinMode(ENA, OUTPUT);
    pinMode(ir_izda, INPUT);
    pinMode(ir_dcha, INPUT);

}


/***   Loop  ***/
void loop() {
    Sensor_izda = digitalRead(ir_izda);
    sensor_dcha = digitalRead(ir_dcha);
    if (((Sensor_izda == sensor_dcha) && (Sensor_izda > 0))) {
        avanza();
    } else if ((Sensor_izda > sensor_dcha)) {
        izquierda();
    } else if ((Sensor_izda < sensor_dcha)) {
        derecha();
    } else if (((Sensor_izda == sensor_dcha) && (Sensor_izda == 0))) {
        parar();
    }
}