Dynamixel RX-28サーボをArduinoで制御（その2）

備忘のため、Dynamixel RX-28サーボの初期化とID変更の手続きをアップしておきます。ただし、実行確認はしていません。

//----------------------------------------------
// モータIDを出荷時設定に初期化する
// 注意：接続しているすべてのモータが対象(IDブロードキャスティング)
//----------------------------------------------
void RX28_reset() {
int sum; // チェックサム
unsigned char buf[10]; // 通信用バッファ

buf[0] = 0xFF; // 固定
buf[1] = 0xFF; // 固定
buf[2] = (unsigned char)0xFE; // IDブロードキャスト: 全サーボ
sum = buf[2]; // チェックサム計算開始
buf[3] = (unsigned char) (2); // データ長 (ID+SUM)
sum += buf[3];
buf[4] = 0x06; //インストラクション リセット
sum += buf[4];
buf[5] = (unsigned char)(~(sum) & 0xFF); // チェックサム

for(int i = 0; i < 6  ; i++){
 Serial.write(buf[i]);
 delay(1); // Arduinoではなぜか必要
}
}

//----------------------------------------------
// モータIDを変更する
// int mID;  新ID
// 注意：接続しているすべてのモータが対象(IDブロードキャスティング)
//----------------------------------------------
void RX28_id_renum(int mID) {
int sum; // チェックサム
unsigned char buf[10]; // 通信用バッファ

buf[0] = 0xFF; // 固定
buf[1] = 0xFF; // 固定
buf[2] = (unsigned char)0xFE; // IDブロードキャスト: 全サーボ
sum = buf[2]; // チェックサム計算開始
buf[3] = (unsigned char) (2 + 2); // データ長 (ID+SUM)
sum += buf[3];
buf[4] = 0x03; // インストラクション コントロールテーブルへの書き込み(即時反映)
sum += buf[4];
buf[5] = (unsigned char) ADDRESS_ID; // コントロールテーブルでのモータIDのアドレス
sum += buf[5];
buf[6] = (unsigned char) mID; // 新ID
sum += buf[6];
buf[7] = (unsigned char)(~(sum) & 0xFF); // チェックサム

for(int i = 0; i < 8 ; i++){
 Serial.write(buf[i]);
 delay(1);
}

};