Kondo servo motors daisy-chain

前回にkondoサーボモータの1個だけを制御する方法を紹介しました．今回は複数のサーボモータの制御し方を紹介したいと思います．
 サーボモータの出荷時すべてIDが0であるため，複製のサーボを使う場合はIDを書き換える必要があります．Arduinoマイコンの場合ソースはモータのIDを書き換えるプログラムは次のようになります．接続し方は前回と同じようになります．

Fig.1 Kondo Servo motor connect to the Arduino MEGA

void setup() {
  Serial.begin(115200,SERIAL_8E1);
   delay(500);
}
byte buf[4];
void loop() {
  buf[0]=0b11100001;//0b111xxxxxは書き換えるためのコマンド，xxxxxは指定するID（例では1とした）
  buf[1]=0x01;
  buf[2]=0x01;
  buf[3]=0x01;
 Serial.write(buf,4);
 delay(1000);
}

IDを書き換えた後，2個のサーボモータの制御プログラムは次のようになります．(ID=0とID=1)

　　　Fig.2 複数サーボの接続

(http://kondo-robot.com/product/03072)

void setup() {
  Serial.begin(115200,SERIAL_8E1);
  Serial3.begin(115200);
   delay(500);
}
int k=-20,k1=-20;
byte buf[3];
char buf_1[6];
int target_angle=9000; //ID0サーボの目標角度 
 int target_angle1=4000;//ID1サーボの目標角度 
int incomingByte[6],incomingByte1[6];
void loop() {
  buf[0]=0b10000001;//servo id 1
  buf[1]=(target_angle1>>7)&0x7f;
  buf[2]=target_angle1&0x7f;
 Serial.write(buf,3);
 Serial.flush();
 if (Serial.available() > 0) {
     for (int n=0;n<6;n++){
       incomingByte[n]=Serial.read();
     }
   }
  delay(1);

  buf[0]=0b10000000; //servo id 0

  buf[1]=(target_angle>>7)&0x7f;
  buf[2]=target_angle&0x7f;
  Serial.write(buf,3);
  Serial.flush();
   if (Serial.available() > 0) {
     for (int n=0;n<6;n++){
       incomingByte1[n]=Serial.read();
     }
   }
  int angle =(incomingByte[4]<<7)|incomingByte[5];
  int angle1 =(incomingByte1[4]<<7)|incomingByte1[5];
  Serial3.print(angle);
  Serial3.print("\t");
  Serial3.print(angle1);
  Serial3.print("\n");
  target_angle+=k;
  target_angle1+=k1;
  if(target_angle<4000){
    k=20;
  }
  else if (target_angle>10000){
    k=-20;
  }
   if(target_angle1<4000){
    k1=20;
  }
  else if (target_angle1>10000){
    k1=-20;
  }
  delay(20);
}