     case TURN_HEAD: {
       turnHead(value);  
       break;
+    }
+    
+    case SET_MOTOR_LEFT: {
+      setMotorSpeed(1, value);  
+      break;
     } 
+    
+    case SET_MOTOR_RIGHT: {
+      setMotorSpeed(2, value);  
+      break;
+    } 
+    
+    case SET_MOTORS: {
+      setMotorSpeed(0, value);  
+      break;
+    } 
 
     default: {
-
+      setMotorSpeed(0,0);
+      turnHead(SERVO_CENTER);
     }
   } 
   
   return readSomething;
 }
+
+void setMotorSpeed(int motor, int value) {
+  motor = constrain(motor, 0, 2);
+  value = constrain(value, -255, 255);
+  int absValue = abs(value);
+  
+  uint8_t direction = RELEASE;
+  if( value > 0 ) {
+    direction = FORWARD;
+  }
+  else if( value < 0 ) {
+    direction = BACKWARD;  
+  }
+  
+  if( motor == 0 ) {
+    motorLeft.setSpeed(absValue);
+    motorRight.setSpeed(absValue);  
+    motorLeft.run(direction);
+    motorRight.run(direction);    
+  }
+  else if( motor == 1 ) {
+    motorLeft.setSpeed(absValue);
+    motorLeft.run(direction);    
+  }
+  else {
+    motorRight.setSpeed(absValue);
+    motorRight.run(direction);  
+  }
+} 
 
 void sonarPing() {
   unsigned int dist = getDistance();

  Serial.begin(BAUD_RATE); 
  lastServoTime = millis();
  lastSonarTime = millis();
  sayYes();
}



enum COMMAND {
  NONE = -1, 
  TURN_HEAD, // value = servo microseconds
  SONAR,  
  SET_MOTOR_LEFT,    // value -255 - 255
  SET_MOTOR_RIGHT,
  SET_MOTORS, 
  COMMAND_MAX
} currentCommand;

