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);
}