#include <Servo.h>

#define PIN_TRIG 4
#define PIN_ECHO 5
#define PIN_PWM 3

Servo servo;

void setup() {
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

  servo.attach(PIN_PWM);
  //Serial.begin(9600);
}

void loop() 
{
//обработка дальномера
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  double distance = pulseIn(PIN_ECHO, HIGH)/58.0;
  //2   - 180
  //400 - 0
  servo.write(round((400 - distance)*180/398));
  //Serial.println(servo.read());
  delay(10);
}