// C++ code
//
#include <Servo.h>

int d = 0;

Servo servo_3;

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_0;

void setup()
{
  servo_3.attach(3, 500, 2500);
  Serial.begin(9600);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  servo_0.attach(0, 500, 2500);

  servo_3.write(90);
}

void loop()
{
  d = 0.01723 * readUltrasonicDistance(7, 8);
  Serial.println(d);
  if (d < 10) {
    servo_3.write(0);
    digitalWrite(4, HIGH);
    digitalWrite(5, LOW);
    digitalWrite(6, LOW);
  }
  else if (d >= 10 && d < 50) {
    servo_0.write(90);
    digitalWrite(4, LOW);
    digitalWrite(5, HIGH);
    digitalWrite(6, LOW);
  }
  else if (d >= 50) {
    servo_3.write(180);
    digitalWrite(4, LOW);
    digitalWrite(5, LOW);
    digitalWrite(6, HIGH);
  }
  delay(10); // Delay a little bit to improve simulation performance
}