FutabaサーボのS.BUS解析３（マイコンからS.BUS経由でサーボを動かす）

　RC用Futabaサーボをマイコンからシリアル通信したくなったのでS-BUSという通信のプロトコル解析をしてみた。前回記事で解析したS.BUS経由でマイコン（RX220）からサーボを動かす。

f:id:kirikoshokunin:20210315191629j:plain:w300

！注意！　このブログで紹介した内容を実施する場合、個人の責任の範囲内で行ってください。当方は責任は負いません。

送信データの作成

　前回、S.BUSで通信するためにには負論理で下図のように目標値を送信すればよいと分かった。

　 f:id:kirikoshokunin:20210314165442p:plain

　マイコンから指令するためには、角度[deg.]から目標値のデータに変換するほうが今後便利。そのため、どのように計算すればよいかをまず考える。まず最初に、サーボの動作角度を調整する必要がある。フタバサーボは基本的に最大±90 deg. が最大っぽいので、今回は±90 deg. と設定した。その時のサーボ角度とバス上のデータの表を下に示す。

S-Link指令値	バス上データ	サーボ角度
2120	1984	90 deg.
1520	1024	0 deg.
920	64	-90 deg.

　数式で示すと目標角度 $ \theta $ に対してサーボモータの最大角度範囲 $ \alpha $ , 指令値（バス上データ）を $ y $ とすると

$$ y= \frac{ 1920 }{ \alpha } (\theta+90)+64 $$

$ \alpha $ は±90deg.と設定した場合 180 となる。

　ちなみに最大角度を左右非対称に設定した場合、0 deg. の指令値は変わらず最大角度の指令値も変わらないので、目標角度に対して指令値に対する係数も左右で変える必要がある。そのため上式を左右で定数項を変えて計算する必要がある。

f:id:kirikoshokunin:20210315140747p:plain:w300 — S-Link　最大角度を左右非対称で設定した場合

S.BUS送信回路（RX220）

　S.BUSは負論理で構成しないといけないので実現するために↓の選択肢が考えられる。

RX220の場合SCI12の拡張シリアル通信を使用
通常のSCI端子の外部にトランジスタやNOT回路を付ける
GPIOで頑張る

　「GPIOで頑張る」は9600 bps なら現実的だが100 kbps で実現するためにはクロック周波数が高いマイコンを使用する必要がある。（RX220でギリギリ？）まぁ、マイコンのハードウェアで実現できることはCPUをできるだけ使わないで、実装する主義なので今回は拡張シリアル通信とSCI端子とトランジスタで実現する。その際の回路を下図に示す。

f:id:kirikoshokunin:20210315155514p:plain:h200 — 拡張シリアル端子を使用した際の回路

f:id:kirikoshokunin:20210315155610p:plain:h200 — 通常のシリアル端子を使用した際の回路

マイコンプログラム

　SBUS経由でサーボを動作させるソースコードを下に示す。力業で送信データを作成してあるので、ID０～６までしか個別に送信できない。また関数内で角度を指示しているので実際に使用するときは配列のポインタ渡しにするか、target_angleをグローバル変数にするかが必要。　このプログラムを動作させるときは、SCI12の極性反転ビットを１にして負論理としている。他のSCIで送信するときは上述した回路を接続して論理反転する必要がある。

/*SBUSサーボ指令角度送信用配列*/
char sbus_data[25] = {
    0x0f,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00,
    0x00,0x00,0x00,0x00,0x00
};

/*-------------------------------------------*/
/* 　void sbus_tx( void )   　　　　　　 */
/*　-sbus送信用関数　　　　　　　*/
/*　-この関数の内部で指令角を定義 　　*/
/*　S.BUS CH ID1~ID6 　に送信する　*/
/*　-送信完了後に再度呼び出される前提*/
/*-------------------------------------------*/
void sbus_tx( void ){
     int i=0;

    /*IDごとにサーボ指令角格納用配列*/
     float  target_angle[16];　　//指令角度格納配列 unit[deg.]
     short  sbus_servo_id[16]      //送信用指令角度格納配列（SBUS用変換後）

     target_angle[0]= 90.0; //ID1の動作角度[deg.]を設定
     target_angle[1]= 90.0; //ID2の動作角度[deg.]を設定
     target_angle[2]= 90.0; //ID3の動作角度[deg.]を設定
     target_angle[3]= 90.0; //ID4の動作角度[deg.]を設定
     target_angle[4]= 90.0; //ID5の動作角度[deg.]を設定
     target_angle[5]= 90.0; //ID6の動作角度[deg.]を設定
     for(i=0;i<=15;i++){
             sbus_servo_id[i] =(short)(  10.667 * (target_angle[i]+90.0)+64.0); //動作角度[deg.]をSBUS用に変換
     }

    /*　sbus_dataに変換した角度データを代入　*/
    sbus_data[1] =  sbus_servo_id[0] & 0x00ff;
    sbus_data[2] = ((sbus_servo_id[0]>>8) & 0x07 )| (sbus_servo_id[1]  <<3 );
    sbus_data[3] = ((sbus_servo_id[1]>>5) & 0x3f ) | (sbus_servo_id[2]  <<6);
    sbus_data[4] = ((sbus_servo_id[2]>>2) & 0xff ) ;
    sbus_data[5] = ((sbus_servo_id[2]>>10) & 0x01 )|(sbus_servo_id[3]  <<1 )   ;
    sbus_data[6] = ((sbus_servo_id[3]>>7) & 0x0f )|(sbus_servo_id[4]  <<4 )   ;
    sbus_data[7] = ((sbus_servo_id[4]>>4) & 0x7f )|(sbus_servo_id[5]  <<7 )   ;
    sbus_data[8] = ((sbus_servo_id[5]>>1) & 0xff )   ;
    sbus_data[9] = ((sbus_servo_id[5]>>9) & 0x03 ) ;

　　sci12_put_data ( sbus_data ,sizeof(sbus_data));　//SCI12で配列に格納されているデータを送信する関数
}