Kondo servo motors daisy-chain

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

                                 

Sponsored Links: