Joystick with servos

#include <Servo.h>
Servo myservo;  // create servo object to control a servo

#define analogInputX A0
#define analogInputY A1
#define pulseinInputS1 A2
#define pulseinInputS2 A3
#define servo1Pin A4
#define servo2Pin A5

//Parameters
#define motor_speedF 255 // Motor speed for forward
#define MOTOR_SPEED_BASE 200 // Base motor speed
#define motor1Speed 255
#define motor2Speed 255
#define xError 120
#define yError 40
#define xCenterValue 512
#define yCenterValue 512
#define PULSE_THRESHOLD 0.2 // pulse width minimum value in ms
/* Motor states by joystick position
FORWARD - M1&M2 forward
BACKWARD -  M1&M2 backward
FORWARD_L -  M1 backward M2 forward // duplicate with BACKWARD_R
FORWARD_R - M1 forward M2 backward // duplicate with BACKWARD_L
BACKWARD_L - M1 forward M2 backward
BACKWARD_R - M1 backward M2 forward
*/
#define NEUTRAL 0
#define FORWARD 1
#define BACKWARD 2
#define FORWARD_L 3
#define FORWARD_R 4
#define BACKWARD_L 5
#define BACKWARD_R 6
#define PULSE_READ_STATE 7 //testing

float xDiff =0;
float yDiff =0;
float x,y=0;
byte state = 0;
float motor_speed;
float motor_diff;
float motor_speed_diff;
float left_motor_speed, right_motor_speed = 0;

unsigned long readingPulse1 = 0;
unsigned long readingPulse2 = 0;
int pulseinCheck = 0; // reading pulse availability
void setup() {
  Serial.begin(9600);
  Serial.println("START ---------------------------");
  myservo.attach(servo1Pin);
  delay(30);
  myservo.write(0);
  delay(30);
  myservo.detach();
  delay(30);
  myservo.attach(servo2Pin);
  delay(30);
  myservo.write(0);
  delay(30);
  myservo.detach();
  delay(30);
}
int pulseinRead()
{
readingPulse1 = pulseIn(pulseinInputS1, HIGH);
//readingPulse2 = pulseIn(pulseinInputS2, HIGH);
readingPulse2 = 0;
// pulseIn() returns value in microseconds we work with miliseconds
Serial.print("reading pulse 1 i 2  ");
Serial.print(readingPulse1);
Serial.print(" | ");
Serial.print(readingPulse2);
Serial.println("---");


if((readingPulse1>PULSE_THRESHOLD) || (readingPulse2 >PULSE_THRESHOLD))
{
    Serial.print("Vliza pri ifa za pulsove ");
    Serial.print(readingPulse1);
    Serial.print(" ");
    Serial.print(readingPulse2);
    Serial.println();
    Serial.println("Vliza reading pulse2 90gr");
    myservo.attach(servo1Pin);
    delay(20);
    myservo.writeMicroseconds(readingPulse1);
    delay(200);
    myservo.detach();
    delay(30);
    /*
    if((readingPulse1 >=500) && (readingPulse1<=560))
    {
      myservo.attach(servo1Pin);
      myservo.write(0);
      myservo.detach();
    }
    if((readingPulse2 >=500) && (readingPulse2<=560))
    {
      myservo.attach(servo2Pin);
      myservo.write(0);
        delay(50);
      myservo.detach();
      delay(30);
    }
    if((readingPulse1 >=1400) && (readingPulse1<=1500))
    {
      Serial.println("Vliza reading pulse1 90gr");
      myservo.attach(servo1Pin);
      delay(120);
      myservo.write(90);
      delay(150);
      myservo.detach();
      delay(100);
    }
    if((readingPulse2 >=1400) && (readingPulse2<=1500))
    {

      Serial.println("Vliza reading pulse2 90gr");
      myservo.attach(servo2Pin);
      delay(20);
      myservo.write(90);
      delay(100);
      myservo.detach();
      delay(30);
    }
    if((readingPulse1 >=2400) && (readingPulse1<=2500))
    {
      myservo.attach(servo1Pin);
      myservo.write(180);
      delay(50);
      myservo.detach();
      delay(30);
    }
    if((readingPulse2 >=2400) && (readingPulse2<=2500))
    {
      myservo.attach(servo2Pin);
      myservo.write(180);
      delay(50);
      myservo.detach();
      delay(30);
    }*/
      state = PULSE_READ_STATE;
   return 1;
  }
  else
  {
    state = NEUTRAL;
    return 0;
  }
}

void servoWrite(int pin,int degr)
{
  myservo.attach(pin);
  delay(120);
  myservo.write(degr);
  delay(150);
  myservo.detach();
  delay(100);
}
void testPrint()
{

  reading();
  Serial.println();
  Serial.print("The value of X is: ");
  Serial.print(x);
  Serial.println();
  Serial.print("The value of Y is: ");
  Serial.print(y);
  Serial.println("------");
  Serial.println();
  delay(5);
}
void reading()
{
   x = analogRead(analogInputX);
   y = analogRead(analogInputY);
   y= map(y,0,1023,1023,0); // maping the Y value because Y is reversed
}
void stateFinding()
{

   if((x>=xCenterValue-xError && x<=xCenterValue+xError)&& (y>=yCenterValue-yError && y<=yCenterValue+yError))
   {
      state = NEUTRAL;
   }
    else if(y>=yCenterValue+yError && x>=xCenterValue-xError && x<=xCenterValue+xError)
   {
    state = FORWARD;
   }
   else if(y<=yCenterValue-yError && x>=xCenterValue-xError && x<=xCenterValue+xError)
   {
    state = BACKWARD;
   }
    else if(y>=yCenterValue+yError && x<=xCenterValue-xError)
   {
    Serial.println("Vliza tuk pri FL");
    state = FORWARD_L;
   }
   else if(y>=yCenterValue+yError && x>=xCenterValue+xError)
   {
    state = FORWARD_R;
   }
   else if(y<=yCenterValue-yError && x<=xCenterValue-xError)
   {
    state = BACKWARD_L;
   }
    else if(y<=yCenterValue-yError && x>=xCenterValue+xError)
   {
    state = BACKWARD_R;
   }
   else
   {
    state = NEUTRAL;
   }

}
void motor1(float LEFT_MOTOR_SPEED)
{
  if(LEFT_MOTOR_SPEED>= 255) LEFT_MOTOR_SPEED= 255;
  //TODO
  delay(20);
}
void motor2(float RIGHT_MOTOR_SPEED)
{

  if(RIGHT_MOTOR_SPEED>= 255) RIGHT_MOTOR_SPEED = 255;
  //TODO
  delay(20);
}


void loop()
{

   if(pulseinCheck==0) // If there aren't impulses avaiable
   {
   reading();
   stateFinding();
   testPrint();
   yDiff = abs(yCenterValue -y);
   xDiff = abs(xCenterValue -x);
   }
   pulseinCheck = 0;
   switch(state)
   {
   case FORWARD:
    servoWrite(servo1Pin, 0);
    servoWrite(servo2Pin, 0);

    motor_speed = (yDiff/255);
    left_motor_speed = motor_speedF; // for debug
    right_motor_speed = motor_speedF;
    motor1(motor_speedF);
    motor2(motor_speedF);
    break;

   case FORWARD_L: // M1 backward M2 forward
    servoWrite(servo1Pin, 30);
    servoWrite(servo2Pin, 90);
    yDiff = map(yDiff, 0,512,0,255);
    xDiff = map(xDiff, 0,512,0,255);

    motor_speed_diff =  (yDiff/255);
    right_motor_speed = MOTOR_SPEED_BASE*motor_speed_diff;
    left_motor_speed = MOTOR_SPEED_BASE*motor_speed_diff;
    left_motor_speed = -left_motor_speed;

    motor1(left_motor_speed);
    motor2(right_motor_speed);
    break;


    case FORWARD_R: // M1 forward M2 backward
    servoWrite(servo1Pin, 90);
    servoWrite(servo2Pin, 180);
    yDiff= map(yDiff,0,512,0,255);
    xDiff = map(xDiff, 0,512,0,255);

    motor_speed_diff =  (yDiff/255);
    right_motor_speed = MOTOR_SPEED_BASE*motor_speed_diff;
    left_motor_speed = MOTOR_SPEED_BASE*motor_speed_diff;
    right_motor_speed = -right_motor_speed;

    motor1(left_motor_speed);
    motor2(right_motor_speed);
    break;

   case BACKWARD_L: // M1 forward M2 backward
    servoWrite(servo1Pin, 90);
    servoWrite(servo2Pin, 180);
    yDiff= map(yDiff,0,512,0,255);
    xDiff = map(xDiff, 0,512,0,255);

    motor_speed_diff = (yDiff/255);
    right_motor_speed = MOTOR_SPEED_BASE*motor_speed_diff;
    left_motor_speed = MOTOR_SPEED_BASE*motor_speed_diff;
    right_motor_speed = -right_motor_speed;

    motor1(left_motor_speed);
    motor2(right_motor_speed);
    break;


   case BACKWARD_R: // M1 backward M2 forward
    servoWrite(servo1Pin, 30);
    servoWrite(servo2Pin, 90);
//    servoWrite(240);
    yDiff = map(yDiff, 0,512,0,255);
    xDiff = map(xDiff, 0,512,0,255);
    motor_diff = xDiff/255;
    motor_speed_diff =  (yDiff/255);
    right_motor_speed = MOTOR_SPEED_BASE*motor_speed_diff;
    left_motor_speed = MOTOR_SPEED_BASE*motor_speed_diff;
    left_motor_speed = -left_motor_speed; // M1 backward

    motor1(left_motor_speed);
    motor2(right_motor_speed);
    break;

   case BACKWARD:
    servoWrite(servo1Pin, 180);
    servoWrite(servo2Pin, 180);
    left_motor_speed = MOTOR_SPEED_BASE*(-1);
    right_motor_speed = MOTOR_SPEED_BASE*(-1);
    break;

   case NEUTRAL:
    servoWrite(servo1Pin, 90);
    servoWrite(servo2Pin, 90);
   left_motor_speed = 0;
   right_motor_speed = 0;
   pulseinCheck = pulseinRead();
   if(pulseinCheck == 0) // If there are no impulses avaible, motors to 0
   {
    motor1(0);
    motor2(0);
   }
    break;

   }
  Serial.println();
  Serial.print("The state is:  ");
  Serial.println(state);
  /*
  Serial.print("The left motor speed is: ");
  Serial.print(left_motor_speed);
  Serial.println();
  Serial.print("The rightt motor speed is: ");
  Serial.print(right_motor_speed);
  Serial.println("------");
  delay(500);
  */
  delay(10);
}