#include <Servo.h>
#include <Ultrasonic.h>

Servo myservo;  // Create a servo object
Ultrasonic ultrasonic(7, 6); // trig pin and echo pin respectively
int servoPin = 9;  // Choose the digital pin connected to the servo

Servo myServo;

void setup() {
  myservo.attach(servoPin);
  Serial.begin(9600);
}

void loop() {
  for (int angle = 0; angle <= 180; angle++) {
    myservo.write(angle);
    delay(100);

    int distance = ultrasonic.read();
    // trig to get x and y values based on the distance in cm of the output
    float x = distance * cos(radians(angle));
    float y = distance * sin(radians(angle));

    // Output X and Y in CSV format
    Serial.print(x);
    Serial.print(",");
    Serial.println(y);
  }
  // If its too slow for you change this :)
  delay(10000);

  for (int angle = 180; angle >= 0; angle--) {
    myservo.write(angle);
    delay(100);

    int distance = ultrasonic.read();
    float x = distance * cos(radians(angle));
    float y = distance * sin(radians(angle));

    // Output X and Y in CSV format
    Serial.print(x);
    Serial.print(",");
    Serial.println(y);
  }

  delay(1000);
}