Trying to do a line follower that can avoid obstacle by doing a rutine, is like doing a half moon around a bottle the problem comes when it reaches the other side, if the robot stops on the line then it does the line follower part and finishes the circuit but if for some reasons it doesnt reach it or goes through it then it doesnt do anything even the part of the 3 sensors that its supposed to move the robot backwards to detect the line its just freezes idk why, and sorry for my english.
#define SensorIzq A8 //sensor left
#define SensorDer A9 //sensor right
#define SensorCen A10 //center sensor
#define PWMI 3 //pwm right motor
#define MiA 8 //left motor forward
#define MiR 9 //left motor backwards
#define PWMD 5//pwm left motor
#define MdA 10//right motor forward
#define MdR 11//right motor backwards
long tiempoC;
int disparadorC = 48; // triger
int entradaC = 49; // echo
float distanciaC;
int evadiendo = 0;
void setup() {
// put your setup code here, to run once:
pinMode(SensorIzq, INPUT);
pinMode(SensorDer, INPUT);
pinMode(SensorCen, INPUT);
pinMode(MiA, OUTPUT);
pinMode(MiR, OUTPUT);
pinMode(MdA, OUTPUT);
pinMode(MdR, OUTPUT);
pinMode(disparadorC, OUTPUT);
pinMode(entradaC, INPUT);
Serial.begin(9600);
}
void Motores(int D1, int D2, int D3, int D4, int PWM1, int PWM2) {
digitalWrite(MiA, D1);
digitalWrite(MiR, D2);
digitalWrite(MdA, D3);
digitalWrite(MdR, D4);
analogWrite(PWMI, PWM1);
analogWrite(PWMD, PWM2);
}
void loop() {
if (evadiendo == 0) {
digitalWrite(disparadorC, HIGH);
delayMicroseconds(10);
digitalWrite(disparadorC, LOW);
tiempoC = (pulseIn(entradaC, HIGH) / 2);
distanciaC = float(tiempoC * 0.0343);
if (distanciaC <= 6) {
Motores(LOW, LOW, LOW, LOW, 0, 0);
evadiendo = 1;
} else {
// put your main code here, to run repeatedly:
int Si = digitalRead(SensorIzq);
int Sd = digitalRead(SensorDer);
int Sc = digitalRead(SensorCen);
delay(5);
if (Si == LOW && Sd == LOW && Sc == HIGH ) {
Motores(HIGH, LOW, HIGH, LOW, 70, 70);
Serial.println("Avanza");
}
else if (Si == HIGH && Sd == LOW) {
Serial.println("Izq");
Motores(HIGH, LOW, LOW, HIGH, 70, 0);
} else if (Si == LOW && Sd == HIGH) {
Serial.println("Der");
Motores(LOW, HIGH, HIGH, LOW, 0, 70);
}
else if (Si == HIGH && Sd == HIGH && Sc == HIGH ) {
Serial.println("Atras");
Motores(LOW, HIGH, LOW, HIGH, 70, 70);
}
}
} else {
digitalWrite(disparadorC, HIGH);
delayMicroseconds(10);
digitalWrite(disparadorC, LOW);
// medimos el pulso de respuesta
tiempoC = (pulseIn(entradaC, HIGH) / 2);
distanciaC = float(tiempoC * 0.0343);
Serial.print("tiempoC=");
Serial.println(tiempoC);
if (evadiendo == 1) {
Motores(LOW, LOW, LOW, LOW, 0, 0);
delay(200);
Motores(HIGH, LOW, LOW, HIGH, 65, 65);
delay(400);
Motores(HIGH, LOW, HIGH, LOW, 65, 65);
delay(300);
Motores(LOW, LOW, HIGH, LOW, 0, 65);
delay(500);
Motores(HIGH, LOW, HIGH, LOW, 65, 65);
delay(800);
Motores(LOW, HIGH, HIGH, LOW, 65, 65);
delay(400);
Motores(HIGH, LOW, HIGH, LOW, 65, 65);
delay(600);
evadiendo = 0;
}
}
}