POP-7 Machanum Wheel Gripper and RC Remote Control

#include <pop7.h>   // POP-7 Board
int Count = 0;
void setup() {
  OK();
  Serial.begin(115200);
  pinMode(A0,INPUT);
  pinMode(A1,INPUT);
  pinMode(A2,INPUT);
  pinMode(A3,INPUT);
  servo(2,170);
}

void loop() {
  int Turning_Wheel = pulseIn(A0, HIGH);
  int Throttle_Trigger = pulseIn(A1, HIGH);
  int servo1 = pulseIn(A2, HIGH);
  int servo2 = pulseIn(A3, HIGH);

  int Throttle = 0+((Throttle_Trigger - 1500)/4); //หาค่าเฉลี่ยสำหรับตัวควบคุมเดินหน้า-ถอยหลัง
  int Turning  = 0+((Turning_Wheel - 1500)/4);    //หาค่าเฉลี่ยตัวหมุนเลี้ยวซ้าย-ขวา
  int SERVO1   = 80 + ((servo1 - 1000)/10);       //หาค่าเฉลี่ยเซอร์โวสำหรับยกขึ้น-ลง

  Throttle = Throttle * -1;

  if(Throttle > 100){ Throttle = 100; }           //กำหนดให้ค่าควบคุมเดินหน้า-ถอยหลังอยู่ระหว่าง -100 ถึง 100
  else if(Throttle < -100){ Throttle = -100; }

  if(Turning > 100){ Turning = 100; }              //กำหนดให้ค่าควบคุมเลี้ยวซ้าย-ขวาอยู่ระหว่าง -100 ถึง 100
  else if(Turning < -100){Turning = -100;}

  if(SERVO1 < 80){SERVO1 = 80;}                    //กำหนดให้ตัวยกมีองศาระหว่าง 80 - 160 องศา
  if(SERVO1 >160){SERVO1 = 160;}

  servo(1,SERVO1); //ตัวยกขึ้น-ลง

//  Serial.print(A);Serial.print("\t");
//  Serial.print(B);Serial.print("\t");
//  Serial.print(C);Serial.print("\t");
//  Serial.print(D);Serial.print("\t");
//  Serial.print(E);Serial.print("\t");
//  Serial.print(servo2);Serial.print("\t");
//  Serial.print(F);Serial.println("\t");

  if((Turning_Wheel  > 1400 && Turning_Wheel < 1600) && (Throttle_Trigger  > 1400 && Throttle_Trigger < 1600) ){
     motor(1,0);motor(2,0);motor(3,0);motor(4,0);       //STOP
  }
  else if(Throttle_Trigger  < 1400 || Throttle_Trigger > 1600){
     motor(1,Throttle);motor(2,Throttle);motor(3,Throttle);motor(4,Throttle);  // FORWARD AND BACK
  }
  else if(Turning_Wheel  < 1400  ){
     motor(1,Turning);motor(2,Turning);motor(3,Turning*-1);motor(4,Turning*-1); //SPIN LEFT
  }
  else if(Turning_Wheel  > 1600 ){
     motor(1,Turning);motor(2,Turning);motor(3,Turning*-1);motor(4,Turning*-1); //SPIN RIGHT
  }
  else if(Turning_Wheel  < 1400 &&  Throttle_Trigger  < 1400 ){
     motor(1,Throttle-40);motor(2,Throttle-40);motor(3,Throttle);motor(4,Throttle); //TURN LEFT
  }
  else if(Turning_Wheel  > 1600 &&  Throttle_Trigger  < 1400 ){
     motor(1,Throttle);motor(2,Throttle);motor(3,Throttle-40);motor(4,Throttle-40); //TURN LEFT
  }

  if(servo2 > 1500 ){   //ควบคุมเซอร์โวสำหรับหนีบ
    Count = Count+1;
    delay(200);
    if(  Count == 1){
      servo(2,69);
    }
    else if(Count > 1){
      servo(2,170);
      Count = 0;
    }
  }


}