   unsigned long diff = now - lastServoTime;
   if( diff < SERVO_WAIT_TIME_MILLIS ) {
     unsigned long delayTime = SERVO_WAIT_TIME_MILLIS - diff;
-    Serial.write("delaying servo movement for ms: ");
-    Serial.print(delayTime, DEC);
     delay(delayTime);
   }
 }
 
-void printDistance() {
+void printOK() {
+  Serial.println("ok"); 
+}
+
+unsigned int getDistance() {
   unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
-  Serial.print(uS / US_ROUNDTRIP_CM); // Convert ping time to distance in cm and print result (0 = outside set distance range)
+  unsigned int distance = uS / US_ROUNDTRIP_CM; // Convert ping time to distance in cm and print result (0 = outside set distance range)
+  return distance;
 }
 
 

  
  switch(command) {
    case SONAR: {
       sonarPing(value);
	   break;
    } 

	default: {
          
	}
  } 
  
  return readSomething;
}

void sonarPing(int microseconds) {
  microseconds = constrain(microseconds, SERVO_MIN_MS, SERVO_MAX_MS);
  servo.writeMicroseconds(microseconds);
  waitForServo();
  lastServoTime = millis();
  printDistance(); 
  lastServoTime = millis();
}

void waitForServo() {

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

#define BAUD_RATE 115200
#define SERVO_HEAD 10
#define TRIGGER_PIN  A1  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     A0  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 300 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
