#include <Servo.h>

const int trigPin =  10;
const int echoPin =  11;
const int servPin =  12;
const int servMin =  15; // degrees sweep minimum
const int servMax = 165; // degrees sweep maximum

// Variables for the duration and the distance
long duration;
int distance;
Servo myServo; // Creates a servo object for controlling the servo motor

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.begin(115200);
  myServo.attach(servPin);
}
void loop() {
  moveServo();
}

void moveServo() {
  for (int j = 0; j < 2; j++) { // two passes
    for (int i = servMin; i <= servMax; i++) { // angle sweep
      myServo.write ((j) ?  180 - i : i);
      delay(30); // allow servo to catch up to code
      distance = calculateDistance();  // get distance
      Serial.print(0); // low limit
      Serial.print(",");
      Serial.print(400); // high limit
      Serial.print(",");
      Serial.print((j) ? 2 * (180 - i) : 2 * i); // angle sweep for plotter
      Serial.print(",");
      Serial.print(distance); // object
      Serial.print(".");
      Serial.println();
    }
  }
}

int calculateDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.017; // 0.034 / 2
  return distance;
}