IPST-SE JOY-PSX Control Motor and Gripper

#include <ipst.h> // IPST-SE Board
#include <PS2X_lib.h>
#define PS2_DAT  16
#define PS2_CMD  17
#define PS2_SEL  18
#define PS2_CLK  19
int error;
int x,a=85,b=0;
PS2X psx;
void setup(){
  pinMode(16,INPUT_PULLUP);
  pinMode(17,INPUT_PULLUP);
  pinMode(18,INPUT_PULLUP);
  pinMode(19,INPUT_PULLUP);
  setTextSize(2);glcdMode(1);
  glcd(0,0,"OK->Bluetooth");
  glcd(1,0,"SW1->JOY PSX");
  while(1){
    if(sw1()){joy();}
    if(sw_OK()){blue();}
  }
}
void loop(){}
void blue(){
  glcdClear();
  setTextSize(3);glcdMode(1);
  setTextColor(GLCD_BLUE);
  glcd(1,0,"Bluetooth");
  pinMode(2,INPUT_PULLUP);
  while(!sw1()){
    if (uart1_available()) {
       x=uart1_getkey();
      if(x==0x38){fd(100);}
      else if(x==0x32){bk(100);}
      else if(x==0x34){sl(100);}
      else if(x==0x36){sr(100);}
      else {ao();}
    }
  }
}
void joy(){
  glcdClear();
  setTextSize(3);glcdMode(1);
  setTextColor(GLCD_YELLOW);
  glcd(1,0,"JOY PSX");
   while(1){
    if (!psx.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, false, false)){
      setTextColor(GLCD_RED);setTextSize(2);
      glcd(3,0,"Connected ");
      break;
    }
   }
   while(!sw_OK()){
    psx.read_gamepad(0,0);    // Read PSX
    if(psx.Button(PSB_PAD_UP)){fd(100);}
    else if(psx.Button(PSB_PAD_DOWN)){bk(100);}
    else if(psx.Button(PSB_PAD_LEFT)){sl(100);}
    else if(psx.Button(PSB_PAD_RIGHT)){sr(100);}
    else if(psx.Button(PSB_L1)){keep();delay(1000);}
    else if(psx.Button(PSB_L2)){push();delay(1000);}
    else if(psx.Button(PSB_TRIANGLE)){sPush();}
    else if(psx.Button(PSB_CROSS)){sPull();}
    else if(psx.Button(PSB_SQUARE)){sKeep();}
    else if(psx.Button(PSB_CIRCLE)){sRel();}
    else{ao();}
  }
}
void sHome(){
  servo(0,85);a=85;servo(1,0);b=0;
  delay(1000);
 // servo(0,-1);servo(1,-1);
}
void keep(){
  servo(0,150);delay(300);
  servo(1,150);b=150;delay(300);
  servo(0,85);a=85;delay(300);
 // servo(0,-1);servo(1,-1);
}
void push(){
  servo(0,150);delay(300);
  servo(1,60);delay(300);
  servo(0,85);a=85;delay(300);
  servo(1,0);b=0;delay(300);
  //servo(0,-1);servo(1,-1);
}
void sPush(){
  if(a>85){a--;servo(0,a);delay(10);}
}
void sPull(){
  if(a<150){a++;servo(0,a);delay(10);}
}
void sKeep(){
  if(b<150){b++;servo(1,b);delay(10);}
}
void sRel(){
  if(b>0){b--;servo(1,b);delay(10);}
}


/*
ยก 85 //
วาง 150
คีบ 150
กางสุด 0
กางกว้าง 60
  PSB_SELECT      0x0001
  PSB_L3          0x0002
  PSB_R3          0x0004
  PSB_START       0x0008
  PSB_PAD_UP      0x0010
  PSB_PAD_RIGHT   0x0020
  PSB_PAD_DOWN    0x0040
  PSB_PAD_LEFT    0x0080
  PSB_L2          0x0100
  PSB_R2          0x0200
  PSB_L1          0x0400
  PSB_R1          0x0800
  PSB_TRIANGLE    0x1000
  PSB_CIRCLE      0x2000
  PSB_CROSS       0x4000
  PSB_SQUARE      0x8000


*/