LABORATORIO 14

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(); 

}

VIDEO DEMOSTRACIÓN