PROGRAMACIÓN DEL CARRITO COMO EVASOR DE OBSTÁCULOS
CÓDIGO FUENTE
int Echo = A4;
int Trig = A5;
int in1 = 9;
int in2 = 8;
int in3 = 7;
int in4 = 6;
int ENA = 10;
int ENB = 5;
int ABS = 130;
int rightDistance = 0,leftDistance = 0,middleDistance = 0;
void Up()
{
analogWrite(ENA,ABS);
analogWrite(ENB,ABS);
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
}
void Down()
{
analogWrite(ENA,ABS);
analogWrite(ENB,ABS);
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
}
void Izq()
{
analogWrite(ENA,ABS);
analogWrite(ENB,ABS);
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
digitalWrite(in3,HIGH);
digitalWrite(in4,LOW);
}
void Der()
{
analogWrite(ENA,ABS);
analogWrite(ENB,ABS);
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
}
void Stop()
{
digitalWrite(ENA,LOW);
digitalWrite(ENB,LOW);
Serial.println("Stop!");
}
/*Ultrasonic distance measurement Sub function*/
int Distance_test()
{
digitalWrite(Trig, LOW);
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(20);
digitalWrite(Trig, LOW);
float Fdistance = pulseIn(Echo, HIGH);
Fdistance= Fdistance/58;
return (int)Fdistance;
}
bool ang;
void setup()
{
ang=1;
pinMode(Echo, INPUT);
pinMode(Trig, OUTPUT);
pinMode(in1,OUTPUT);
pinMode(in2,OUTPUT);
pinMode(in3,OUTPUT);
pinMode(in4,OUTPUT);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
Stop();
}
void loop()
{
middleDistance = Distance_test();
if(middleDistance<=25)
{
Stop();
delay(500);
if(ang){
Der();
delay(700);
}
else{
Izq();
delay(700);
}
ang = not ang;
}
Up();
}
