// C++ code

//

#include <Servo.h>

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

{

pinMode(2, OUTPUT);

pinMode(3, OUTPUT);

pinMode(4, OUTPUT);

pinMode(0, OUTPUT);

servo_4.attach(4, 500, 2500);

digitalWrite(2, LOW);

digitalWrite(3, LOW);

digitalWrite(4, LOW);

digitalWrite(0, LOW);

}

void loop()

{

if (0.01723 * readUltrasonicDistance(7, 6) < 10) {

digitalWrite(2, HIGH);

digitalWrite(3, LOW);

digitalWrite(0, LOW);

servo_4.write(90);

} else {

servo_4.write(0);

digitalWrite(2, LOW);

digitalWrite(3, HIGH);

}

delay(10); // Delay a little bit to improve simulation performance

}