 
 
 //computing Motor velocity VS an X,Y control vector
-float compute_Right_Motor_Raw_Speed(float value_x, float value_y){
+float compute_Right_Motor_Raw_Velocity(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){
+float compute_Left_Motor_Raw_Velocity(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){
+#ifndef TEST_XY_ONLY
+float compute_Up_Motor_Raw_Velocity(float value_z){
   return (Z_factor*value_z);
 }
+#endif
 
 //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_Right_Motor_Velocity(float value_x, float value_y){
+    return map(compute_Right_Motor_Raw_Velocity(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_Left_Motor_Velocity(float value_x, float value_y){
+  return map(compute_Left_Motor_Raw_Velocity(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);
+#ifndef TEST_XY_ONLY
+float compute_Up_Motor_Velocity(float value_z){
+  return map(compute_Up_Motor_Raw_Velocity(value_z), min_up, max_up,-MOTOR_MAX_PWM,MOTOR_MAX_PWM);
 }
+#endif
 
-
-//this function is used to enable the use of compute_XXX_Motor_Speed 
+//this function is used to enable the use of compute_XXX_Motor_Velocity 
 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);   
+   min_right = compute_Right_Motor_Raw_Velocity(-100.0, -100.0);
+   max_right = compute_Right_Motor_Raw_Velocity(100.0, -100.0);
+   min_left = compute_Left_Motor_Raw_Velocity(-100.0, 100.0);
+   max_left = compute_Left_Motor_Raw_Velocity(100.0, 100.0);   
+#ifndef TEST_XY_ONLY
+   min_up = compute_Up_Motor_Raw_Velocity(-100.0);
+   max_up = compute_Up_Motor_Raw_Velocity(100.0);   
+#endif
 }
 
 


#define MOTOR_MAX_PWM 255.0

const int motor_right_forward = 7;
const int motor_right_backward = 8;
const int motor_right_enable = 3;

const int motor_left_forward = 13;
const int motor_left_backward = 12;
const int motor_left_enable = 11;

const int motor_up_forward = 5;//TODO set the adequate value
const int motor_up_backward = 6;//TODO set the adequate value
const int motor_up_enable = 10;//TODO set the adequate value


byte motor_right_pwm = 0;
byte motor_left_pwm = 0;
byte motor_up_pwm = 0;

float motor_right_speed = 0;
float motor_left_speed = 0;
float motor_up_speed = 0;

char incomingByte_X = 0;
char incomingByte_Y = 0;
char incomingByte_Z = 0;

const float X_factor = 1.0;
const float Y_factor = 1.0;
const float Z_factor = 1.0;

float min_right;
float max_right;
float min_left;
float max_left;
float min_up;
float max_up;

const long interval = 200;     
long previousMillis = 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
}

