#include <Servo.h>
#include <NewPing.h>

#define TRIGGER_PIN  4  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     3  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

int servoPin=6;

int servoIncrement=1;
int servoPos=0;
int radarDistance=0;

Servo servo;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

unsigned long currentMillis=0;
unsigned long servoPreviousMillis=0;
const unsigned long servoPeriod=15;
unsigned long pingPreviousMillis=0;
const unsigned long pingPeriod=1000;

void radarping(int& radarDistance) {

  if (currentMillis-pingPreviousMillis >= pingPeriod) {
    pingPreviousMillis=currentMillis;

  radarDistance=sonar.ping_cm();
    }
}

void servoSweep(int& servoPos) {

if (currentMillis-servoPreviousMillis >= servoPeriod) {
  
  servoPreviousMillis=currentMillis;

  servoPos += servoIncrement;

  if ((servoPos >180) || servoPos <0) {
    servoIncrement= -servoIncrement;
  }

servo.write(servoPos);

}

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

 void loop() {

  currentMillis=millis();

  servoSweep(servoPos);
  radarping(radarDistance);
  Serial.print("Ping: ");
  Serial.print(radarDistance); 
  Serial.print("cm");
  Serial.print(" Servo Position: ");
  Serial.println(servoPos);
 }