« 「週刊 ロボゼロ」 第6号(1) | トップページ | ロボゼロでも使えるBluetoothモジュール「VS-BT001」が発売 »

Arduinoでロボゼロのサーボを動かしてみよう!(9)

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

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

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

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

という手順で行います。

  • サーボのIDはアドレス4に書かれてる
  • サーボ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;
}

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

two通信速度の変更の改良
さて、第4回で作成した通信速度の変更の為のスケッチですが、サーボの1番でなければ使えないので、他の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;
}

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

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

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

----------------
にほんブログ村 科学ブログ ロボットへ
にほんブログ村

|

« 「週刊 ロボゼロ」 第6号(1) | トップページ | ロボゼロでも使えるBluetoothモジュール「VS-BT001」が発売 »

Arduino / Processing」カテゴリの記事

週刊 ロボゼロ」カテゴリの記事

コメント

baudrateの型がintになっていますけど、intだと115200まで扱えないのでlongにすべきだと思います。
サーボのID変更ができずに、ハマってしまいました。
でも、とても参考になりこのブログの影響でArduinoを始めてロボゼロの制作もとても楽しめいています。
これからもよろしくお願いします。

投稿: Eiji | 2011年4月30日 (土) 22時39分

Eijiさん
ありがとうございます。
intはsint16なんですね。うっかりしてました(汗)
後で、修正します。

Arduino簡単にあれこれできて楽しいですよね。サーボのことだけでなく、他のことでもあれこれ利用できるといいですよね。。

投稿: micono | 2011年4月30日 (土) 23時48分

コメントを書く



(ウェブ上には掲載しません)




トラックバック

この記事のトラックバックURL:
http://app.cocolog-nifty.com/t/trackback/221673/51334554

この記事へのトラックバック一覧です: Arduinoでロボゼロのサーボを動かしてみよう!(9):

« 「週刊 ロボゼロ」 第6号(1) | トップページ | ロボゼロでも使えるBluetoothモジュール「VS-BT001」が発売 »