     incomingByte_X= constrain(incomingByte_X, -100,100);
     incomingByte_Y = Serial.read();
     incomingByte_Y= constrain(incomingByte_Y, -100,100);
+#ifndef TEST_XY_ONLY 
     incomingByte_Z = Serial.read();
     incomingByte_Z= constrain(incomingByte_Z, -100,100);
-    
-    //computing new speed for wheels  
+#endif    
+    //computing new desired velocity for each motor
+    right_motor_velocity = compute_Right_Motor_Velocity(float(incomingByte_X), float(incomingByte_Y));
+    left_motor_velocity = compute_Left_Motor_Velocity(float(incomingByte_X), float(incomingByte_Y));
+#ifndef TEST_XY_ONLY     
+    up_motor_velocity = compute_Up_Motor_Velocity(float(incomingByte_Z));
+#endif
     //configuring right motor PWM
-    motor_right_pwm = byte(abs(motor_right_speed));
-    if(motor_right_speed >= 0){
-        digitalWrite(motor_right_forward, HIGH);
-        digitalWrite(motor_right_backward, LOW);
+    right_motor_pwm = byte(abs(right_motor_velocity));
+    if(right_motor_velocity >= 0){
+        digitalWrite(RIGHT_MOTOR_FORWARD, HIGH);
+        digitalWrite(RIGHT_MOTOR_BACKWARD, LOW);
     }
     else{
-        digitalWrite(motor_right_forward, LOW);
-        digitalWrite(motor_right_backward, HIGH);
+        digitalWrite(RIGHT_MOTOR_FORWARD, LOW);
+        digitalWrite(RIGHT_MOTOR_BACKWARD, HIGH);
     }
     //configuring left motor PWM
-    motor_left_pwm = byte(abs(motor_left_speed));  
-    if(motor_left_speed >= 0){
-        digitalWrite(motor_left_forward, HIGH);
-        digitalWrite(motor_left_backward, LOW);
+    left_motor_pwm = byte(abs(left_motor_velocity));  
+    if(left_motor_velocity >= 0){
+        digitalWrite(LEFT_MOTOR_FORWARD, HIGH);
+        digitalWrite(LEFT_MOTOR_BACKWARD, LOW);
     }
     else{
-        digitalWrite(motor_left_forward, LOW);
-        digitalWrite(motor_left_backward, HIGH);
+        digitalWrite(LEFT_MOTOR_FORWARD, LOW);
+        digitalWrite(LEFT_MOTOR_BACKWARD, HIGH);
     }
-    //configuring up motor PWM
-    motor_up_pwm = byte(abs(motor_up_speed));  
-    if(motor_up_speed >= 0){
-        digitalWrite(motor_up_forward, HIGH);
-        digitalWrite(motor_up_backward, LOW);
+#ifndef TEST_XY_ONLY     
+   //configuring up motor PWM
+    up_motor_pwm = byte(abs(up_motor_velocity));  
+    if(up_motor_velocity >= 0){
+        digitalWrite(UP_MOTOR_FORWARD, HIGH);
+        digitalWrite(UP_MOTOR_BACKWARD, LOW);
     }
     else{
-        digitalWrite(motor_up_forward, LOW);
-        digitalWrite(motor_up_backward, HIGH);
+        digitalWrite(UP_MOTOR_FORWARD, LOW);
+        digitalWrite(UP_MOTOR_BACKWARD, HIGH);
     }
+#endif
     //some debugging info
-    //Serial.write(motor_right_pwm);
-    //Serial.write(motor_left_pwm);
-    //Serial.write(motor_up_pwm);
+    //Serial.write(right_motor_pwm);
+    //Serial.write(left_motor_pwm);
+    //Serial.write(up_motor_pwm);
     Serial.flush();   
   }
 }



//computing Motor velocity VS an X,Y control vector
float compute_Right_Motor_Raw_Speed(float value_x, float value_y){
  return (X_factor*value_x - Y_factor*value_y*sign(value_x));
}

float compute_Left_Motor_Raw_Speed(float value_x, float value_y){
  return (X_factor*value_x + Y_factor*value_y*sign(value_x));
}

float compute_Up_Motor_Raw_Speed(float value_z){
  return (Z_factor*value_z);
}

//computing Motor velocity VS an X,Y control vector
float compute_Right_Motor_Speed(float value_x, float value_y){
    return map(compute_Right_Motor_Raw_Speed(value_x, value_y), min_right, max_right,-MOTOR_MAX_PWM,MOTOR_MAX_PWM);
}

float compute_Left_Motor_Speed(float value_x, float value_y){
  return map(compute_Left_Motor_Raw_Speed(value_x, value_y), min_left, max_left,-MOTOR_MAX_PWM,MOTOR_MAX_PWM);
}

float compute_Up_Motor_Speed(float value_z){
  return map(compute_Up_Motor_Raw_Speed(value_z), min_up, max_up,-MOTOR_MAX_PWM,MOTOR_MAX_PWM);
}


//this function is used to enable the use of compute_XXX_Motor_Speed 
void compute_Resize_Parameters(){
   min_right = compute_Right_Motor_Raw_Speed(-100.0, -100.0);
   max_right = compute_Right_Motor_Raw_Speed(100.0, -100.0);
   min_left = compute_Left_Motor_Raw_Speed(-100.0, 100.0);
   max_left = compute_Left_Motor_Raw_Speed(100.0, 100.0);   
   min_up = compute_Up_Motor_Raw_Speed(-100.0);
   max_up = compute_Up_Motor_Raw_Speed(100.0);   
}



 //initialize Serial communication
 Serial.begin(9600);
 Serial.flush();
 //initializing pins used to control motors power
 pinMode(RIGHT_MOTOR_FORWARD, OUTPUT);
 pinMode(RIGHT_MOTOR_BACKWARD, OUTPUT);
 pinMode(LEFT_MOTOR_FORWARD, OUTPUT);
 pinMode(LEFT_MOTOR_BACKWARD, OUTPUT);
//initializing pins used to control motors rotation clockwise or trigonometricwise
 digitalWrite(RIGHT_MOTOR_ENABLE, LOW);
 digitalWrite(LEFT_MOTOR_ENABLE, LOW);

#ifndef TEST_XY_ONLY 
 pinMode(UP_MOTOR_FORWARD, OUTPUT);
 pinMode(UP_MOTOR_BACKWARD, OUTPUT);
 digitalWrite(UP_MOTOR_ENABLE, LOW);
#endif
 
 compute_Resize_Parameters();
}

void loop() {
  
    //continuously controlling motors by applying the computed pwm
    analogWrite(RIGHT_MOTOR_ENABLE, right_motor_pwm);
    analogWrite(LEFT_MOTOR_ENABLE, left_motor_pwm);
    analogWrite(UP_MOTOR_ENABLE, up_motor_pwm);
    delay(100);//control loop = 10Hz
}

