#include <Servo.h>

const int p_sonar_trig = 7;
const int p_sonar_echo = 8;
const int p_servo_control_signal = 9;
Servo canard_1; 

void setup() {
  Serial.begin(9600);
  pinMode(p_sonar_trig,OUTPUT);
  pinMode(p_sonar_echo, INPUT);
  canard_1.attach(p_servo_control_signal);
}

void loop() {

  long pulse_duration_ms, distance_in_cm;

  send_sonar_pulse();
  pulse_duration_ms = pulseIn(p_sonar_echo, HIGH);
  distance_in_cm = convert_ms_to_cm(pulse_duration_ms);
  canard_1.write(map(distance_in_cm, 0, 400, 0, 180)); 

  delay(50);
}

long convert_ms_to_cm(long microseconds) {
  // Speed of sound = 29 microseconds per centimeter.
  return microseconds / 29 / 2;
}

bool send_sonar_pulse() {
  digitalWrite(p_sonar_trig, LOW);
  delayMicroseconds(2); 
  digitalWrite(p_sonar_trig, HIGH);
  delayMicroseconds(10); 
  digitalWrite(p_sonar_trig, LOW);
}