   
   switch(command) {
     case SONAR: {
-       sonarPing(value);
-	   break;
+      sonarPing();
+      break;
+    }
+    
+    case TURN_HEAD: {
+      turnHead(value);  
+      break;
     } 
 
-	default: {
-          
-	}
+    default: {
+
+    }
   } 
   
   return readSomething;
 }
 
-void sonarPing(int microseconds) {
+void sonarPing() {
+  unsigned int dist = getDistance();
+  Serial.print(dist);
+  Serial.print("\n");
+  printOK();
+}
+
+void turnHead(int microseconds) {
   microseconds = constrain(microseconds, SERVO_MIN_MS, SERVO_MAX_MS);
   servo.writeMicroseconds(microseconds);
   waitForServo();
   lastServoTime = millis();
-  printDistance(); 
-  lastServoTime = millis();
+  printOK();
 }
 
 void waitForServo() {

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

#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.

void setup() {
  
  // turn on servo
  servo.attach(SERVO_HEAD);   
  Serial.begin(BAUD_RATE); 
  lastServoTime = millis();
  lastSonarTime = millis();
  sayYes();
