Arduinoでロボゼロのサーボを動かしてみよう！（９）

前回、ロボゼロサーボ用のシールドを作ってみましたが、サーボを複数個使った場合の為に「サーボのIDを変更してみたい」と思います

これま作ってきたスケッチは、サーボのIDは１番と決め打ちしたスケッチでしたが、１番でない場合も利用出来るように作り替えた方いいですね。

サーボのIDを変更する
４回目で通信速度の変更を行いましたが、それと同じように

1. サーボのIDを変更し、
2. ロムに書きこんで、
3. リスタートする

という手順で行います。

• サーボのIDはアドレス４に書かれてる
• サーボID書き換えコマンドの後は、新しいIDで操作する。
• パケットのサーボIDを変更する為の関数を作った。IDの変更およびそれに伴うチェックサムの変更を行う。
  void setServoID(byte pkt[], byte n, byte id) {
    pkt[2]=id;
    calcCheckSum(pkt,n);
  }
• ID変更後、回転させてみて、変更できたかどうか確認できるようにした。

ということで、以下です。

※通信速度用の変数の宣言でintをlongに修正しました（2011/3/30)
//-------------------------------------
// サーボIDを変更し、サーボを動かす
// サーボが動けば、変更成功！
//
// シグナル＝デジタルポート1番
// 2011/4/8
// Copyright 2011 Micono Utilities
//-------------------------------------

// ID A を Bに変更
byte IDAtoB[] = {0xFA, 0xAF, 0x0A, 0x00, 0x04, 0x01, 0x01, 0x0B, 0x05};
// ROM Write
byte RomWrite[] = {0xFA, 0xAF, 0x0A, 0x40, 0xFF, 0x00, 0x00, 0xBE};
// Servo Restart
byte ServoreStart[] = {0xFA, 0xAF, 0x0A, 0x20, 0xFF, 0x00, 0x00, 0xDE};

// romwriterestart ロム書き込み＋再起動
//推奨されてないやり方、自己責任で
byte romwriterestart[] = {0xFA, 0xAF, 0x0A, 0x60, 0xFF, 0x00, 0x00, 0x9E};

// Torque ON
byte trqOn[] = {0xFA, 0xAF, 0x01, 0x00, 0x24, 0x01, 0x01, 0x01, 0x24};
// Rotate90
byte cw90[] = {0xFA, 0xAF, 0x01, 0x00, 0x1E, 0x02, 0x01, 0x84, 0x03, 0x9B};
// Rotate-90
byte ccw90[] = {0xFA, 0xAF, 0x01, 0x00, 0x1E, 0x02, 0x01, 0x7C, 0xFC, 0x9C};

void setup() {
  pinMode(13,OUTPUT);    //LED 使用
  digitalWrite(13,HIGH); //LED ON 設定開始
 
  long  baudrate=115200;  //通信速度
  byte now_id=1;         //現在のID
  byte new_id=3;         //変更するID
 
  Serial.begin(baudrate);  //通信速度を設定
  delay(500);
 
  IDAtoB[7]=new_id;                    //変更するID
  setServoID(IDAtoB,9,now_id);         //現在のID
  Serial.write(IDAtoB,9);              //IDを変更する
 
  //ID書き換えコマンド後、新しいIDで動作する
  setServoID(RomWrite,8,new_id);       //変更するID
  setServoID(ServoreStart,8,new_id);   //変更するID
  setServoID(romwriterestart,8,new_id);//変更するID

  Serial.write(RomWrite,8);      //変更したID
  delay(2000);                   //しばらく待つ
  Serial.write(ServoreStart,8);  //変更したID
  //Serial.write(romwriterestart,8);//現在のIDのままで行う
 
  delay(1000);
  setServoID(trqOn,9,new_id);  //ID変更
  setServoID(cw90,10,new_id);  //ID変更
  setServoID(ccw90,10,new_id); //ID変更
  Serial.write(trqOn,9);       //トルクON
 
  digitalWrite(13,LOW);  //LEF OFF 設定終了
}

void loop() {
  delay(2000);
  Serial.write(cw90,10);
  delay(2000);
  Serial.write(ccw90,10);
}

void setServoID(byte pkt[], byte n, byte id) {
  pkt[2]=id;
  calcCheckSum(pkt,n);
}

void calcCheckSum(byte pkt[], byte n) {    // チェックサムの設定
  int sum = 0;
  for(int i=2; i<n-1; i++){
    sum = sum ^ pkt[i];
  }
  pkt[n-1]=sum;
}

//-------------------------------------

通信速度の変更の改良
さて、第４回で作成した通信速度の変更の為のスケッチですが、サーボの１番でなければ使えないので、他のIDでも通信速度を変更できるように改良してみました。

• 通信速度の変更の方は、変更コマンド送信後、再構築（ServoStart）を行うまで、変更前の通信速度になるので注意
• 通信速度の更後、回転させてみて、変更できたかどうか確認できるようにした。

//-----------------------------------
// 通信速度を変更して、サーボを動かす
// サーボが動けば、変更成功！
//
// シグナル＝デジタルポート1番
// 2011/4/8
// Copyright 2011 Micono Utilities
//-----------------------------------

// baudrate
byte NewBaudRate[] = {0xFA, 0xAF, 0x01, 0x00, 0x06, 0x01, 0x01, 0x07, 0x03};
// ROM Write
byte RomWrite[] = {0xFA, 0xAF, 0x01, 0x40, 0xFF, 0x00, 0x00, 0xBE};
// Servo Restart
byte ServoreStart[] = {0xFA, 0xAF, 0x01, 0x20, 0xFF, 0x00, 0x00, 0xDE};

// romwriterestart ロム書き込み＋再起動
//推奨されてないやり方、自己責任で
byte romwriterestart[] = {0xFA, 0xAF, 0x01, 0x60, 0xFF, 0x00, 0x00, 0x9E};

// Torque ON
byte trqOn[] = {0xFA, 0xAF, 0x01, 0x00, 0x24, 0x01, 0x01, 0x01, 0x24};
// Rotate90
byte cw90[] = {0xFA, 0xAF, 0x01, 0x00, 0x1E, 0x02, 0x01, 0x84, 0x03, 0x9B};
// Rotate-90
byte ccw90[] = {0xFA, 0xAF, 0x01, 0x00, 0x1E, 0x02, 0x01, 0x7C, 0xFC, 0x9C};

void setup() {
  pinMode(13,OUTPUT);    //LED 使用
  digitalWrite(13,HIGH); //LED ON 設定開始
 
  long now_baud=115200;  //現在の通信速度
  long new_baud=38400;   //変更する通信速度
  byte id=3;            //現在のサーボID
 
  //通信速度を設定
  Serial.begin(now_baud); 
  delay(500);
 
  //変更する通信速度
  NewBaudRate[7]=BaudRateToIndex(new_baud);
 
  //サーボIDを変更して、チェックサムを再計算
  setServoID(NewBaudRate,9,id);
  setServoID(RomWrite,8,id);
  setServoID(ServoreStart,8,id);
  //setServoID(romwriterestart,8,id);
 
  //通信速度変更後、再起動するまで、最初の通信速度のままで動作する
  Serial.write(NewBaudRate,9);  //通信速度変更
  Serial.write(RomWrite,8);     //書き込み処理
  delay(2000);                  //しばらく待つ
Serial.write(ServoreStart,8); //再起動
  //Serial.write(romwriterestart,8);

  delay(1000);                 //しばらく待つ
  Serial.end();                //通信終了
  Serial.begin(new_baud);      //新しい通信速度で起動
  delay(500);
 
  setServoID(trqOn,9,id);      //ID変更
  setServoID(cw90,10,id);      //ID変更
  setServoID(ccw90,10,id);     //ID変更
  Serial.write(trqOn,9);       //トルクON
 
  digitalWrite(13,LOW);  //LEF OFF 設定終了
}

void loop() {
  delay(2000);
  Serial.write(cw90,10);
  delay(2000);
  Serial.write(ccw90,10);
}

int BaudRateFromIndex(byte n) {
  switch(n) {
    case 0: return   9600; break;
    case 1: return  14400; break;
 case 2: return  19200; break;
 case 3: return  28800; break;
 case 4: return  38400; break;
 case 5: return  57600; break;
 case 6: return  76800; break;
 case 7: return 115200; break;
 case 8: return 153600; break;
 case 9: return 230400; break;
  }
}

byte BaudRateToIndex(long n) {
  switch(n) {
    case   9600: return 0; break;
 case  14400: return 1; break;
 case  19200: return 2; break;
 case  28800: return 3; break;
 case  38400: return 4; break;
 case  57600: return 5; break;
 case  76800: return 6; break;
 case 115200: return 7; break;
 case 153600: return 8; break;
 case 230400: return 9; break;
  }
}
            
void setServoID(byte pkt[], byte n, byte id) {
  pkt[2]=id;
  calcCheckSum(pkt,n);
}

void calcCheckSum(byte pkt[], byte n) {    // チェックサムの設定
  int sum = 0;
  for(int i=2; i<n-1; i++){
    sum = sum ^ pkt[i];
  }
  pkt[n-1]=sum;
}

//-----------------------------------

さて、今回は以上です。また次回をお楽しみに

----------------

• 「週刊 ロボゼロ」 第３号（３）Arduinoでサーボのテスト（PWM）
• Arduinoでロボゼロのサーボを動かしてみよう！（２）
• Arduinoでロボゼロのサーボを動かしてみよう！（３）
• Arduinoでロボゼロのサーボを動かしてみよう！（４）
• Arduinoでロボゼロのサーボを動かしてみよう！（５）
• Arduinoでロボゼロのサーボを動かしてみよう！（６）
• Arduinoでロボゼロのサーボを動かしてみよう！（７）
• Arduinoでロボゼロのサーボを動かしてみよう！（８）
• Arduinoでロボゼロのサーボを動かしてみよう！（９）
• Arduinoでロボゼロのサーボを動かしてみよう！（10）
• Arduinoでロボゼロのサーボを動かしてみよう！（11）
• RS30xPacketUtil 1.0.1公開

----------------

にほんブログ村