 
 
 //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
 }
 
 

 //initialize Serial communication
 Serial.begin(9600);
 Serial.flush();
 //initializing pins used to control motors 
 pinMode(motor_right_forward, OUTPUT);
 pinMode(motor_right_backward, OUTPUT);
 pinMode(motor_left_forward, OUTPUT);
 pinMode(motor_left_backward, OUTPUT); 
 pinMode(motor_up_forward, OUTPUT);
 pinMode(motor_up_backward, OUTPUT);
 
 
 digitalWrite(motor_right_enable, LOW);
 digitalWrite(motor_left_enable, LOW);
 digitalWrite(motor_up_enable, LOW);
 compute_Resize_Parameters();
}

void loop() {
  
    //continuously controlling motors
    analogWrite(motor_right_enable, motor_right_pwm);
    analogWrite(motor_left_enable, motor_left_pwm);
    analogWrite(motor_up_enable, motor_up_pwm);
    delay(100);//control loop = 10Hz
}


//general definitions to interface with the hardware 


#define UP_MOTOR_FORWARD 5
#define UP_MOTOR_BACKWARD 6
#define UP_MOTOR_ENABLE 10

#define RIGHT_MOTOR_FORWARD 7
#define RIGHT_MOTOR_BACKWARD 8
#define RIGHT_MOTOR_ENABLE 3

#define LEFT_MOTOR_FORWARD 13
#define LEFT_MOTOR_BACKWARD 12
#define LEFT_MOTOR_ENABLE 11

//general definitions as parameters of the  
#define MOTOR_MAX_PWM 255.0




byte right_motor_pwm = 0;
byte left_motor_pwm = 0;

float right_motor_velocity = 0;
float left_motor_velocity = 0;

char incomingByte_X = 0;
char incomingByte_Y = 0;

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

float min_right;
float max_right;
float min_left;
float max_left;


#ifndef TEST_XY_ONLY
byte up_motor_pwm = 0;
float up_motor_velocity = 0;
char incomingByte_Z = 0;
const float Z_factor = 1.0;
float min_up;
float max_up;
#endif


const long interval = 200;     
long previousMillis = 0;
