#include <Servo.h>
Servo myServo; // Creates a servo object for controlling the servo motor
const int TrigPin = 3; //  Trig / TX
const int EchoPin = 2; //Echo / RX
#define SERVO_PWM_PIN 12 //set servo to Arduino's pin 12

// Variables for the duration and the distance
long duration;
int distance;  

// means -angle .. angle
#define ANGLE_BOUNDS 85
#define ANGLE_STEP 1
 
int angle = 0;
 
// direction of servo movement 
// -1 = back, 1 = forward 
int dir = 1;


void setup() { 
  
 //Initialize 
 Serial.begin(9600);         //  The measurement results through the serial output to the serial port on the PC monitor
 pinMode(EchoPin, INPUT);    //  The set EchoPin input mode.
 pinMode(TrigPin, OUTPUT);   //  The set TrigPin output mode.
 myServo.attach(SERVO_PWM_PIN); //set servo to Arduino's pin 12
 //myServo.write(90);
}  

void loop() {   
  
  delay(50); // speed of the sweep
  // we must renormalize to positive values, because angle is from -ANGLE_BOUNDS .. ANGLE_BOUNDS
  // and servo value must be positive
  myServo.write(angle + ANGLE_BOUNDS);
 
  // read distance from sensor and send to serial
  getDistanceAndSend2Serial(angle);
  
  // calculate angle 
  if (angle >= ANGLE_BOUNDS || angle <= -ANGLE_BOUNDS) {
    dir = -dir;
  }
  angle += (dir * ANGLE_STEP); 
  
 }  

int getDistanceAndSend2Serial(int angle) {
  //angle = map(angle,-ANGLE_BOUNDS,ANGLE_BOUNDS,175,5);
  int cm = calculateDistance();
  Serial.print(angle, DEC); // Output to the serial port monitor 
  Serial.print(",");
  Serial.println(cm, DEC);
  
}
// 
// Function for calculating the distance measured by the Ultrasonic sensor
// 
int calculateDistance(){ 
    // By the Trig / Pin sending pulses trigger US-100 ranging
   digitalWrite(TrigPin, LOW); 
   delayMicroseconds(50);                  //Set the pulse width of 50us (> 10us)
   digitalWrite(TrigPin, HIGH);            // Send pulses begin by Trig / Pin
   delayMicroseconds(50);                  //  wait 50 microseconds for it to return
   digitalWrite(TrigPin, LOW);             // The end of the pulse 

  duration = pulseIn(EchoPin, HIGH); // Reads the echoPin, returns the sound wave travel time in microseconds
  distance= duration*0.034/2;
  return distance;
}