近藤科学のロボットサーボをArduinoで動かしてみよう

近藤科学のKRSシリーズのサーボモータをArduinoで動かしたいと思います。
一般に安く売られているPWMサーボにくらべ、角度取得や強さの変更ができるので、ロボットにとってはこのあたりが重要になってきます。

とりあえず、仕様は、ここを参考に
以前に比べ、ずいぶん見やすくなってそうです。

今回は、角度を指定で動くところと、ストレッチやスピードの変更を行いたいと思います。
ストレッチ：保持特性を変化させる(確か、モータに出力する傾きを変化させる)
スピード：最高回転速度を変化させます(モータの出力上限を変化させる)
だそうです。

回路について

回路は掲載しているものだと信号的に違和感を感じたので、下記のものにしました。

3ステートバッファを用いて送信と受信を切り替えます。
送受信の切り替えは、今回P2を用います。
P2をHにすると送信、Lにすると受信になります。
UARTはUnoではSerial(P0,P1)しかついていないので、そこを使います。
書き込みピンと共用していますので、書き込む時は回路を外します。
SN74LVC1G125を使っていますが、他の物でも問題ないかと思います。
ただし、5Vトレラントの物を使わないと、3.3V駆動のArduinoを使えなくなりますので、注意が必要です。
また、切り替えにNOT回路を用いてもいいのですが、簡易的にデジトラを用いました。

さて、これはICSの信号に関係するPINしか書いていません。
3本線で1番pinにこの信号が入力されます。
(1番ピンは耳がついているほうです)
2番PINはサーボの電源
3番PINはサーボのGNDになります。
電源はArduinoで出力できませんので、別途くべてあげる必要があります。
その電源のGNDとArduinoのGNDは結線しておく必要もあります。

プログラムについて

コマンドに関しては固定値を送りますので、先に定義をしておきます。
また、PINの設定値も定義しておきます。

グローバルで定義

const int EnPin = 2;  //グローバルで定義しておく
//コマンドの定義
const byte ICS_POS_CMD = 0x80;
const byte ICS_PARA_WRITE_COMMND = 0xC0;
const byte SUB_ST_COMMND = 0x01;
const byte SUB_SP_COMMND = 0x02;

初期設定

ICSの通信は、115200bps、8bit、偶パリティ(EVEN)です。
あと、入出力切り替えPINはP2に接続していますので、このPINを出力にします。

setup();

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200, SERIAL_8E1);  //UARTの通信設定
  pinMode(EnPin, OUTPUT);             //送受信切替えピンの設定
  digitalWrite(EnPin, LOW);           //常に受信モードにしておく
}

送受信部分

基本、コマンドを送って受信することを常に行います。
まずは、数byte送信した時数byte返ってくる部分を一連の関数化をします。

Synchronize();

///
/// 送受信する
///
bool Synchronize(byte *txBuff, size_t txLength, byte *rxBuff, size_t rxLength)
{
  int size; //受信したbyte数

  //定義していなかったらエラーにする
  if (Serial == false)return false;

  Serial.flush();

  digitalWrite(EnPin, HIGH);

  Serial.write(txBuff, txLength);

  Serial.flush();   //送信が終わるのを待つ

  digitalWrite(EnPin, LOW);

  size = Serial.readBytes(rxBuff, rxLength);

  if (size != rxLength) //指定した受信数かどうか確認
  {
    return false;
  }
  return true;
}

P2を送信直前にHにして、送信終了したらLにします。
Serial.flush()で待たないとデータを投げただけで送信中にLになります。

もし、つながってない等でデータが指定byte数返ってこない場合はエラーを返します。

角度を指定して動かす

コマンドリファレンスを参考にコマンドを作って送信し、返信データを取得します。
ポジションコマンドに関しては、今現在の位置が返ってきます。
角度は

左端	----	中央	----	右端
3500	...	7500	...	11500
-135度	...	0度	...	135度
となります。
データを渡す等々は、、、解説がめんどくさいので、コマンドリファレンスかプログラムを参考にｗ

ポジションコマンド

///
/// ポジションコマンドを送る
///
int ics_set_pos(byte id,unsigned short pos)
{
  byte tx_data[3];
  byte rx_data[3];
  bool flag;
  int rAngle;   //受信した角度データ
  /// 送信コマンドを作成
  tx_data[0] = ICS_POS_CMD + id;
  tx_data[1] = (byte)((pos & 0x3F80) >> 7);
  tx_data[2] = (byte)(pos & 0x007F);
  
  //送受信を行う
  flag = Synchronize(tx_data,3,rx_data,3);

  if(flag == false) //失敗した場合は-1を返す
  {
    return -1;
  }

  rAngle = ((rx_data[1] << 7) & 0x3F80) + (rx_data[2] & 0x007F);
  
  return rAngle;  
}

IDとポジション(角度)データを渡すとその位置に移動します。
もし、通信がうまくいかなかった場合は、-1が返るようになっています。
また、FREEにする場合は角度データに0を送ります。

関数化したので、あとはこれを呼び出します。
下記は0番のIDを中央(7500)に移動させる例です。

int posData = ics_set_pos(0,7500);

ストレッチ,スピードを変更する

同じように、ストレッチやスピードを変更する関数を作成します。
ストレッチやスピードはサブコマンド部分が違うだけでそれ以外は何気に同じになります。
どちらとも、範囲は1～127になります。

ストレッチを変更する関数

///
/// ストレッチを変更する
///
bool ics_set_st(byte id,byte stData)
{
  byte tx_data[3];
  byte rx_data[3];
  bool flag;
  /// 送信コマンドを作成
  tx_data[0] = ICS_PARA_WRITE_COMMND + id;
  tx_data[1] = SUB_ST_COMMND;
  tx_data[2] = stData;
  
  //送受信を行う
  flag = Synchronize(tx_data,3,rx_data,3);

  if(flag == false) //失敗した場合は-1を返す
  {
    return -1;
  }
  
  return flag;  
}

スピードを変更する関数

///
/// スピードを変更する
///
bool ics_set_sp(byte id,byte spData)
{
  byte tx_data[3];
  byte rx_data[3];
  bool flag;
  /// 送信コマンドを作成
  tx_data[0] = ICS_PARA_WRITE_COMMND + id;
  tx_data[1] = SUB_SP_COMMND;
  tx_data[2] = spData;
  
  //送受信を行う
  flag = Synchronize(tx_data,3,rx_data,3);

  if(flag == false) //失敗した場合は-1を返す
  {
    return -1;
  }  
  return flag;  
}

同じように、この関数を呼び出すことで、各パラメータを変更できます。

下記はID0のストレッチを20にする例です。

  ics_set_st(0,20);

下記はID0のスピードを64にする例です。

ics_set_sp(0,64);

さて、loopのなかでこの関数を呼ぶと簡単に動かせるようになります。

以上、KRSシリーズのサーボモータをArduinoで動かす方法でした。
UARTで割り込みとか使えたら待ち時間を削れるんだけどなぁ。。

あと、サンプルはどうやってあげればいいんだｗｗｗｗ