シーサーちゃんのロボット技術紹介（制御）

2022NHKロボコンに出場した沖縄Bのロボットの技術紹介をします。
僕が担当したのがロボットの回路設計、回路製作、配線製作、プログラムでしたので、そこについて主に書いておこうと思います。

今年の回路とプログラムはここにあります
https://github.com/nalinally/robocon/tree/main/2022NHK

概要

今年のロボットの大まかな説明を行います。
まず、スケルトニクスのようなコントローラを操縦者が背負います。（スケルトニクスについては検索）そして、操縦者が腕を動かすと、ロボットについている7軸（モーターが7つある）のロボットアームが操縦者と同じように動きます。
今年のロボットの概観としては、まず足にタイヤを動かすモーターが４つ、ロボットを展開させるためのパワーウィンドウ（モーターの一種　車の窓の開閉に使われているからパワーウィンドウというらしいです）が４つあり、その上に7軸のロボットハンドが2つ乗っかっています。コントローラ側には、14個のロータリーエンコーダ（軸の回転を検出するセンサ）と6軸ジャイロセンサ（回る速度と加速度を検出するセンサ）が載っていて、このコントローラについているセンサの状態によってロボットの動きが決まります。

開発の時の工夫

回路とかプログラムとか、こうするとサステナブルな感じに開発できたよっていうものをいかに乗せておきます。

STM32CubeMXはいいぞよ

回路が複雑になってきたら、STM32CubeMXというソフトウェアを使うと便利です。このソフトウェアは、マイコンのピンアサインを効率よく決めることができるものです(ほんとはもっといろんなことができるのですが、使いこなせていない...)。ピンアサインというのは、マイコンのどのピンにどの機能を持たせるかを決めたものです。例えば、DigitalInとか、PwmOutとかいうと、MBED勢はなんとなくイメージがつくと思います。実は、MBEDはマイコンの仕組みをよくわかってなくても簡単にコードを書ける一方、ピンアサインの関係で思わぬエラーが出たりします。エラーの例として、PwmOutを使うときにはタイマを使うのですが、同じタイマを共有しているPwmOutピンはPwm周期が同じになります。なので、モータードライバーへのPwm出力とサーボモータへのPwm出力が同じタイマを共有していたりすると、両方が同じPwm周波数になってしまってどちらかがうまく動かなくなります。
そういうエラーを防ぐために、STM32CubeMXを使ってピンアサインを決めます。詳しい使い方は記事にあげたいなあ。気が向いたら。

↑開発画面はこんな感じ

UIは大事

UIというのは、ユーザインタフェースのことで、ロボットの状態を知る手段とロボットに何かしら指令を送る手段をまとめてユーザインタフェースといいます。
例えば、LEDを一個基板に用意しておいて、ある処理をしている間そのLEDが点灯するようにしておくと、ロボットがその処理をしていることを、簡単に知ることができます。また、ボタンを一個基板に用意しておいて、このボタンによってプログラムの中のパラメータを変えられるようにしておくと、いちいちプログラムを書き込まなくてもロボットの調整が可能になります。
回路を設計する段階でどんなUIが必要になるかはあんまわからないので、最低限
・LED2個
・スイッチ2個
・ポテンショ1個
はあるといいかなと思います。
・7セグメントLED
があると最高です。

回路

全体的に工夫した点として、配線と基板のコネクタにすべて番号を振ったのは良かったなと思います。こうすることで、基板間のつなげたいところに番号を振っておけば下級生の子たちも勝手に作業を進めることができます。「この長さでコネクタはこれでこういう配線作って」という風に、いちいち指示しなくていいのがお互い楽ですね。チェック表を作っておけば作っている配線がかぶることもなくなり、名前とか書かせれば品質管理とかもできそうです。また、沖縄高専は毎年基板と配線をすべて取り外して手荷物として輸送する（そうしないと結構な確率で向こうで不具合が起こる）ため、回路の組み立ての際には番号がかなり貢献してくれました。
今年一番やらかしたことは、圧着の方法が間違っていたことです。自分がなんか、ただの圧着だけでは物足りなくなって、はんだでメッキし始めたんですが、あれだめらしいですね、ちゃんと調べて、正しい方法で行いましょう。今年は配線トラブルが一番きつかったなあ...（←戦犯）
回路の実装方法としては、①メーカーに発注、②エッチング、③ユニバーサル基板（なんか等間隔にいっぱい穴空いてるやつ）に実装、の3つの方法で行いました。メーカーさんは、pcbgogoさんに発注しました。今年初めて産学官の予算で回路を発注することに成功したので、一応できるよってことだけ言っておきます。やり方をまとめて記事にするとかは言ってないです。どの実装方法がいいかは、回路の規模によります。小さいなら③、中くらいなら②、大きいなら①という感じです。ただ、メーカーに発注する場合はサイズが100mm*100mmを超えると急に高くなるという点に注意です。

（配線の写真・配線のチェック表の写真）

制御回路

制御回路は3つの回路から構成されています。それぞれ同じマイコン（Nucleo-f446re）を使っていて、回路も同じものを使っています。他の高専を見ると、メインの制御回路が一つあって、その指令でセンサやアクチュエータ（電気で動く部品　モータや電磁弁など）を制御するサブの回路がいくつかぶら下がっている感じがほとんどです。沖縄高専のロボットは、メインの制御回路が3つあって、それぞれの回路がセンサやアクチュエータを直接駆動しています。メインの制御回路を複数にする利点としては、割と開発が早いのと、ハード系のトラブルを起こしている箇所の特定が楽ということぐらいです。あとはデメリットの方が圧倒的に大きいので、ちゃんとメイン回路は1つにしましょう。

T型コネクタからサーボとマイコンへの給電を行っています。ちょっとミスったなと思ったのは、サーボとマイコンの給電は別口にすべきだなと思いました。その方が何かと便利。
こだわりポイントは、XHコネクタを外側に、横向きの物を付けたことです。かっこいいからつけただけだったけど、結果省スペース化にもだいぶ貢献してくれてます。あとは、XHコネクタは取り外しにくいので、結構間隔を開けて配置して抜き差ししやすいようにしました（おかげで結構でかくなった）。
あとは、一種類の回路で3つすべての回路の機能を満たせるようにしました。例えば、モーターを2つ、エンコーダを3つ使う回路と、モーターを4つ、エンコーダを1つ使う回路があった場合、モーターを4つ、エンコーダを3つ使う回路があればその2つの回路の機能をどちらも満たすことができます。それの大規模版みたいなことを遂行しました。基板を発注しようとすると、大体最低5枚での発注になるはずなので、その5枚を有効に活用できるのもこの手法のメリットだと思います。ただ、さっきも言いましたが、メインの制御回路は絶対、一つにすべきです。

右手に使った回路　右側のコネクタとかを使っていない

昇降機構、足回り、首の制御に使った回路　エンコーダ(5ピンのコネクタ)とかを使っていない

駆動回路

駆動回路はモータードライバしかなく、それらはすべて市販のものを使っています。電流をたくさん流すであろう肩軸と肘軸のモータと昇降機構のパワーウィンドウにつなげるモタドラには20Aを流すことができるMD20A、それ以外のモータにはMD10Cを使っています。ただ、このモタドラたちはなんかの保護回路がないらしいです。自分はBチームで何も起きなかったので詳しく知らないですが、Aチームの子がそれが原因でマイコン溶かしまくって泣いてました。モタドラの配置の仕方は縦刺しです。平積みだと下の段のモタドラがちょっといじりにくくなるのと、この方が省スペースでかっこいいという理由です。他高専の方には、「逆縦刺しモタドラは初めて笑」といわれました。縦刺しには10角のフレームを使っています。

遠隔非常停止回路

手動なので遠隔非常停止は必須ではなかったのですが、一応作りました。マイコンはNucleo-F303k8、通信モジュールはXBee Pro S1を使っています。マイコンからフォトカプラ(LEDと光センサを合わせたもの)を駆動して、フォトカプラがリレー(コイルを用いてスイッチを駆動する部品)を駆動して切り替えを行います。発注した基板だと、電気の通り道が細すぎるので、十分な電流を流せませんでした。そこで、あとから配線でピン同士を繋げています。他の高専さんの回路を見ると、配線ではなく、盛りはんだをすることで電流の通り道を太くして電流容量を確保していました。天才かよ！って思いました。

プログラム

今年は3台のNucleoを連携させてロボットを動かしたので、メインプログラムも3つあります。
環境はオンラインのMBEDコンパイラを使っていて、使用言語はC++です。

通信の方式

通信は、以下のフォーマットで通信を行いました。コントローラ側にある17個のエンコーダ（17個まで読めたけど、いろいろあって実際には14個しか読まなかった）、ジャイロセンサの情報を、このフォーマットを使ってロボット側に伝送しています。無線通信の回路には、Xbee pro s1を使いました。ただかなり絶滅危惧種と化しているので、SBDBTとかに移行したほうが良さげかも？

pitch, roll, yawというのはそれぞれ回転方向の向きを表しています。例えば、「右肩」といっても、右肩は2軸で構成されているので、それらを区別するために回転方向を追記しています。

（pitch roll yawの説明）

何やら「/64」や「%64」とかいてありますね。これはプログラムの中での計算機号で、「/」は「÷」、「%」は「割ったときのあまりを求める」という意味です。「/64」は「64で割る」、「%64」は「64で割ったときの余りを求める」という意味です。ではなぜセンサの値を64で割ったり余りを求めているかといいますと、それは一度に送れるデータの大きさに関係しています。

通信方式にはいろいろありますが、今年のこのロボットにはUARTという規格（通信における決まりごと）の通信が用いられています。他の通信方式としては、CAN、SPI、I2Cといったものがあります。そして、UART通信では、一度に送れる最大のデータ数は8bitです。詳しい説明は省きますが、-128から127までのデータしか送れません。しかし、エンコーダのデータの値は、-500から500くらいまでの値をとるので（実際には無限に回せば無限に値は増えていきますが、今回は人間の関節の可動域の分しか回らないためです）、一回で送ることができません。そこで、一旦値を2つにばらして、別々で送って、それを受け取る側（つまりロボット）が組み立てることで、データを伝送しています。実際にコントローラ側で値を2つにばらす部分のプログラムと、ロボット側で値を組み立てる部分のプログラムを記述します。

main.cpp

//コントローラからデータを割込みでうけとる
void JointData(void)
{
    //受信割り込みのときに来るよ

    signed char XBEE_Data;
    static int bits = 0;
    
    //バッファからデータを受け取る
    XBEE_Data = Xbee.getc();

    //アンカーならデータが何番目かを判断する
    if( XBEE_Data == -128 ){
        //PC.printf("\n\r128,");
        bits = start_rh*2 - 1;
        return;
    }
    if( XBEE_Data == -127 ){
        bits = start_lh*2 - 1;
        return;
    }
    if( XBEE_Data == -126 ){
        bits = start_ud*2 - 1;
        return;
    }
    
    //データをコネコネする
    con[bits] = XBEE_Data;
    if(bits >= start_lh*2 && bits <= end_lh*2 && bits % 2 == 0){
        angle[(bits/2)-start_lh] = RtoDeg(Compnum(con[bits-1], con[bits]));
    }
    if(bits == btndata*2){
        btn = Compnum(con[bits-1], con[bits]);
    }
    if(bits >= num_data * 2 + 5){
        bits = 0;
    }
    else {
        bits++;
    }
    
    //バッファを空にする
    while( Xbee.readable() ){ XBEE_Data = Xbee.getc(); }
}

（通信フォーマットの写真）

アンカーと書いてあるのは、自分が勝手に作った用語ですが、文字通り「イカリ」のように参照番号を固定する役目があります。例えば、フォーマット通りに通信していても、途中で通信の取りこぼしがあった場合、ロボット側では最初から何番目のデータかで何のデータかを把握しているため、データが一個ずつずれていって大変なことになります。アンカーは、「このデータは〇番目のデータだよ」というのをロボットに教えることによって、データがずれるのを防ぐことができます。ここで一つ注意なのは、アンカーとして送るデータを、他のデータの時に送らないようにしないといけません。例えば、エンコーダの値が-126の時に素直に-126と送ってしまうと、ロボット側でアンカーだと勘違いされてしまいます。「/128」ではなく「/64」なのはそのためです。

（アンカーの説明）

ロボット側で通信を受け取る時には、受信割り込みというものを使います。データが届くと、バッファというものに一時的にデータが格納されます。郵便ポストのようなイメージです。そしてバッファにデータが入るとマイコンにお知らせがいきます。マイコンはこのお知らせを受け取ると、自分の作業を中断してバッファに入ったデータを取りに行きます。この仕組みが受信割り込みです。

（受信割り込みの説明）

普通に受け取るのとは何が違うのかというと、普通に受け取る場合は、決められた時間ごと、例えば0.01秒毎にバッファにデータが入っていないかをマイコンが確認しに行きます。これだと、例えば0.01秒に2回データが来た時には１回目のデータを取りこぼしてしまうし、１回もデータが来なかった場合にはバッファを見に行った時間が無駄になります。それを防ぐために、バッファにデータが入るたびにマイコンがデータを取りに行くようにしています。

(受信割り込みじゃないときの図)

ここで注意なのは、受信割り込みを設定しているときにバッファからデータがあふれると、永遠に受信割り込みが入ってマイコンさんは自分の作業ができなくなります。受信割り込みを設定するときには、バッファにデータを見に行くときに、ついでにバッファを空にする処理を追加してあげるとよいでしょう。

（バッファがあふれるとこうなる）

ライブラリ

今年はライブラリを自作しました。ライブラリについて、すごく乱暴な説明をすると、たくさんの処理をひとつにまとめたものが関数ですが、たくさんの関数をひとつにまとめたものがライブラリです。メインのプログラムとは別にプログラムファイルを作って、そこに記述してある処理をメインのプログラムから呼び出して実行することができます。こうすることで、メインのプログラムが小さくて済むというのと、よく使うような汎用的な処理を簡単に別のプログラムに移植できます。また、プログラムを適度に階層化することで、見やすいプログラムを作ることができます。ライブラリの作り方については、また別に記事を上げようと思います。気が向いたら。

(ライブラリ説明)

手を制御するプログラム

手を制御するプログラムについて説明します。
ロボットを制御するプログラムは基本的に数値の演算なので
コントローラ側でロータリーエンコーダの値を読んで、その値がどのように演算されて最終的にロボットの腕についているモータやサーボモータへの出力になっているのかを説明していきたいと思います。

まず、コントローラ側のマイコンがコントローラのエンコーダの値を読みます。この時の値はint(整数)型です。
次に、このデータを64で割った商とあまりに分割して、それらをロボットに送信します。
ロボット側で、コントローラから送られてきたデータをもとのエンコーダの値になおします。
この値はあくまでエンコーダからとった値なので、これを角度に変換します。エンコーダの1回転で2048増える(じつはエンコーダの中にスイッチがあって、そのスイッチによって1回転でどれくらい増えるか設定できる)ため、エンコーダの値に(360/2048)をかけると角度の値が得られます。
コントローラの関節は実際の人間の手の関節と比べて可動域が小さいです。なので、値に決められた倍数をかけることでコントローラの動きを増幅してロボットに伝えます。
各関節には可動域があるので、その可動域よりも大きい値だった場合は可動域の上限、小さい値だった場合は可動域の下限に値を設定します。
こうして得られた値と、各関節の現在の角度を比べることで、モータへの出力を決めます。モータがDCモータ(電池につなぐと回るようなモータ)だった場合は、得られた値と、各関節の現在の角度の差に比例した速度でモータを回しています。モータがサーボモータ(位置を指令するとその位置まで回るモータ)だった場合は、得られた値をさらにサーボモータへのパルスに変換したものを出力しています。

ダイレクトティーチングを用いてロボットの動きをプログラミング

ダイレクトティーチングとは、実際に人間がロボットの手を取って動きを教える、というプログラミングの手法です。ここでは、ロボットの手ではなく、コントローラの手を動かし、コントローラからの値をロボットが記憶しておくことで、同じ動きを再現することができます。
先ほどロボットの手を動かすプログラムの説明をしましたが、その中で出てきた、エンコーダの値に(360/2048)をかけることで得られる角度の値を、ロボットのポーズごとに保存します。この保存するタイミングは、人間がスイッチの切り替えによって指定できるようになっています。一連の動きを終えた後に、ロボットに記憶したコントローラからの値を吐かせます。吐かせたデータをコピーして、ロボットの、動きを再生する部分のプログラムに貼り付けます。このプログラムをもう一度ロボットに書き込むことで、動きを記憶したロボットの出来上がりです。記憶した動きは、操縦者がシーサーベルトのボタンを押すことによって再生されるようになっています。

ウォッチドッグタイマ

ウォッチドッグとは、「番犬」という意味です。その名の通り、マイコンがちゃんと動作しているか常に監視し、マイコンがフリーズしたり、どこかで無限ループに入ったりすると、自動でマイコンにリセットをかけてくれます。仕組みとして、まず、マイコンの中にウォッチドッグタイマという名前のタイマがあります。このタイマは常にカウントダウンしていて、このタイマがカウントダウンしきる(0になる)とマイコンにリセットがかかります。プログラムの中に定期的にこのタイマをリセットする処理を入れておくことで、なにもエラーがなければこのタイマはカウントダウンしきる前にリセットされ、何かエラーが起きてマイコンがフリーズしたときにはこのタイマはリセットされずに0になるまでカウントダウンされ、マイコンがリセットされます。
今年のロボットではたまにマイコンがフリーズする原因不明(たぶん通信関係だと思いますが...)のエラーがあったので、本番マイコンがフリーズしてもリトライをしなくてもいいようにこの機能を実装しました。

最後に

これでロボットの説明は終わりです。
できるだけわかりやすく、漏れがないように説明したつもりでしたが、大丈夫だったかな
まあ、めちゃくちゃ疲れながら書いてるので、そこはご愛嬌といいうことで...
書いているうちに別で書きたい記事も出てきたので、気合があれば書きます。

最後まで読んでくれてありがとうございました。この記事が少しでも助けになれば幸いです。