ZX-SERVO16i with Hex Crawler

#include <ipst.h>
// Define Servo Motor Channel
#define M1  2
#define M2  6
#define M3  10
#define M4  3
#define M5  7
#define M6  11
#define U1  0
#define U2  4
#define U3  8
#define U4  1
#define U5  5
#define U6  9
// Define default position
#define mCCW   850
#define mCW    650
#define uUp    1150
#define uDown  280
int i=0;
int Mx[6]={
  2,6,10,3,7,11};
int Ux[6]={
  0,4,8,1,5,9};
char x[20];

void setup(){
  glcdMode(1);
  setTextSize(2);
  ZX_SERVO16_Init();
  delay(2000);
  for(i=0;i<6;i++){
    servoWrite(Ux[i],280);
  }
  for(i=0;i<6;i++){
    servoWrite(Mx[i],750);
  }
  glcd(0,0,"Servo Init");
}
void loop(){
  servoWrite(U1,1150);
  servoWrite(M1,650);
  delay(200);
  servoWrite(U1,280);
  servoRamp(M1,850);
  servoWrite(U4,1150);
  servoWrite(M4,850);
  delay(200);
  servoWrite(U4,280);
  servoRamp(M4,650);
  servoWrite(U2,1150);
  servoWrite(M2,650);
  delay(200);
  servoWrite(U2,280);
  servoRamp(M2,850);
  servoWrite(U5,1150);
  servoWrite(M5,850);
  delay(200);
  servoWrite(U5,280);
  servoRamp(M5,650);
  servoWrite(U3,1150);
  servoWrite(M3,650);
  delay(200);
  servoWrite(U3,280);
  servoRamp(M3,850);
  servoWrite(U6,1150);
  servoWrite(M6,850);
  delay(200);
  servoWrite(U6,280);
  servoRamp(M6,650);
}

void servoRamp(byte CH,int POS){
  byte L,H;
  Serial1.print("!SC");
  Serial1.write(CH);
  Serial1.write(0x15);
  L=lowByte(POS);
  H=highByte(POS);
  Serial1.write(L);
  Serial1.write(H);
  Serial1.write(0x0D);
  Serial1.flush();
  delay(100);
}
void servoWrite(byte CH,int POS){
  byte L,H;
  Serial1.print("!SC");
  Serial1.write(CH);
  Serial1.write(0x00);
  L=lowByte(POS);
  H=highByte(POS);
  Serial1.write(L);
  Serial1.write(H);
  Serial1.write(0x0D);
  Serial1.flush();
  delay(100);
}

void ZX_SERVO16_Init(){
  out(17,0);
  delay(100);
  out(17,1);
  delay(1000);
  Serial1.begin(2400);
  Serial1.print("!SCSBR");
  Serial1.write(1);
  Serial1.write(0x0D);
  delay(100);
  Serial1.begin(38400);

}