ELIMINATORIAS CONCURSO DE ROBOTICA

 // C++ code

//

#include <Servo.h>

int distancia = 0;

long readUltrasonicDistance(int triggerPin, int echoPin)

{

  pinMode(triggerPin, OUTPUT);  // Clear the trigger

  digitalWrite(triggerPin, LOW);

  delayMicroseconds(2);

  // Sets the trigger pin to HIGH state for 10 microseconds

  digitalWrite(triggerPin, HIGH);

  delayMicroseconds(10);

  digitalWrite(triggerPin, LOW);

  pinMode(echoPin, INPUT);

  // Reads the echo pin, and returns the sound wave travel time in microseconds

  return pulseIn(echoPin, HIGH);

}

Servo servo_4;

void setup()

{

  Serial.begin(9600);

  servo_4.attach(4, 500, 2500);

  pinMode(7, OUTPUT);

}

void loop()

{

  distancia = 0.01723 * readUltrasonicDistance(6, 5);

  Serial.println(distancia);

  if (distancia <= 20) {

    servo_4.write(90);

    digitalWrite(7, HIGH);

    delay(1000); // Wait for 1000 millisecond(s)

  } else {

    servo_4.write(0);

    digitalWrite(7, LOW);

    delay(1000); // Wait for 1000 millisecond(s)

  }

}