トランジスタ技術７月号「倒立振子」の制作記録

# 記事の概要
2019年7月号のトランジスタ技術において特集されていたカルマンフィルタを利用した倒立振子を実際に制作しました。
他の方の制作に参考になればと、その制作記録をここに公開します。
作業工程を羅列しているだけなので、技術面の解説は本誌をご覧ください。

# 部品購入
[部品リスト(PDF)](https://shop.cqpub.co.jp/hanbai/books/I/I000321/Kalman-TG1_buhin.pdf)に従って購入しました。

~~期間限定ですが、[現代制御ロボティクス学習キット(限定生産)](https://shop.cqpub.co.jp/hanbai/books/I/I000321.htm)という便利なセットがあるので、そちらを利用するのもいいかもしれません。
あちらこちらを回って部品を集める手間が省け、CQ出版社調達のカップラのような、自力では入手が難しい部品も手に入ります。~~
（2020年7月に販売終了を確認）

## マイコンボード 
<img width="50%" alt="部品_マイコン.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/a7c6dc14-a3e6-72e3-d808-db6a516b2aac.jpeg">
秋月電子にて購入
[ＳＴＭ３２　Ｎｕｃｌｅｏ　Ｂｏａｒｄ　ＳＴＭ３２Ｆ４０１](http://akizukidenshi.com/catalog/g/gM-07723/) 1個

## 加速度センサ・ジャイロセンサなど
<img width="50%" alt="部品_センサ.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/151e5d88-e4f7-a30c-8a06-2de54e9ded7a.jpeg">

- [加速度センサ・ジャイロセンサ AE-BMX055](http://akizukidenshi.com/catalog/g/gK-13010/) 1個（秋月電子）
- [ロータリ・エンコーダ EC202A100A](https://www.sengoku.co.jp/mod/sgk_cart/detail.php?code=3AWB-A5LL) 1個（千石電商）
- [ロータリ・エンコーダ用ケーブル EC202用ハーネス　A150](https://www.sengoku.co.jp/mod/sgk_cart/detail.php?code=EEHD-4B5E) 1個（千石電商）
- [ICソケット6P](http://akizukidenshi.com/catalog/g/gP-08616/) 1個（秋月電子）
- [2回路2接点トグルスイッチ](https://www.sengoku.co.jp/mod/sgk_cart/detail.php?code=EEHD-4HTX) 1個（千石電商）もしくは[2回路2接点トグルスイッチ](http://akizukidenshi.com/catalog/g/gP-00301/) 1個（秋月電子）
- 電解コンデンサ470uF 2個
- 電解コンデンサ2.2uF  1個
- セラミックコンデンサ0.1uF 3個

電解コンデンサ470uFはモータードライバの電源近くに配置するもので、記事中の回路図では220uFでした。
0.1uFと並列に接続されているので合成静電容量は、どちらでもほぼ0.1uFです。パスコンの役割を果たしていると思われます。
わざわざ電解コンデンサを入れる理由が分からずに調べたところ、ノイズ対策やアバランシェ破壊対策の為に、電源の近くになるべく大きいコンデンサを入れるそうです。
[【復刻】ロボコン向けモータドライバの作り方 Vol.4](http://yuqlid.hatenablog.com/entry/2016/03/24/231800)
なるべく大きければいいということで、容量は適当に数百uFを選択すればいいと思われます。

## モータードライバなど
<img width="50%" alt="部品_モータードライバ.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/c98a227b-f15e-265e-5cd0-b7b659dcd17f.jpeg">

- [モータ・ドライバIC TA7291P](http://akizukidenshi.com/catalog/g/gI-02001/) 1セット（2個入り）（秋月電子）
    - （2020年11月追記）生産終了のようです。[「TA7291Pの代わりってないのかしら」](https://dalomo.net/blog/2019/11/02/737/)において代替品が紹介されています。
- [電池ボックス 単三×４本　リードタイプ](http://akizukidenshi.com/catalog/g/gP-02671/) 1個（秋月電子）
- [分割ロングピンソケット　２ｘ４２　（８４Ｐ）](http://akizukidenshi.com/catalog/g/gC-05780/) 2個（秋月電子）

単3電池は1.2Vの充電式ニッケル電池を4本使って4.8V（約5V）にします。1.5Vのアルカリ電池を使用すると電圧が6Vになってしまい、5Vレギュレータが必要になります。

写真の基板はサイズが小さすぎたので、自宅にあった以下の基板を使用しました。
140mm×95mmの片面基板です。

<img width="50%" alt="部品_基板.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/df5d9ae1-4532-c455-98d9-615dcc86d48c.jpeg">

## シャーシ
<img width="40%" alt="部品_基板.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/9e2185a5-7e77-8a5e-4bbb-0e0ccceb6c79.jpeg">

千石電商にて購入
[ユニバーサルプレート　210×160](https://www.sengoku.co.jp/mod/sgk_cart/detail.php?code=EEHD-4UVP) 1個

## ギア・ボックスとタイヤ
<img width="90%" alt="部品_モーター.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/cc9da266-5f58-79ef-db6c-801619cba2ab.jpeg">

- タミヤ　ハイパワーギアボックスHE 2個
- タミヤ　スポーツタイヤセット56㎜径 1個

千石電商 秋葉原本店にて購入。通信販売はしていませんが、Amazonなどでも購入できます。

## XHコネクタ
<img width="60%" alt="部品_XH.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/5e881b35-f3ec-ce61-30fa-4cc104f9e086.jpeg">

細々と揃えるのが面倒だったので、Amazonで[XHコネクタキット](https://www.amazon.co.jp/IZOKEE-460%E5%80%8B-2-54mm-%E3%82%B3%E3%83%8D%E3%82%AF%E3%82%BF-%E3%82%B8%E3%83%A3%E3%83%B3%E3%83%91%E7%B7%9A/dp/B071XV8LRB/ref=sr_1_5?__mk_ja_JP=%E3%82%AB%E3%82%BF%E3%82%AB%E3%83%8A&keywords=XH%E3%82%B3%E3%83%8D%E3%82%AF%E3%82%BF&qid=1562039186&s=gateway&sr=8-5)を購入しました。
実は今回このコネクタは使用していません。モーターと基板を接続するのに使用するはずでしたが、実際には、導線を接ぎ木のように足して、直接基板につけました。

## LED、抵抗など
以下は自宅にある在庫を使用しました。

- LED 赤、青、黄色　各1個
- カーボン抵抗 220Ω 4個
- カーボン抵抗 3.3kΩ 2個
- カーボン抵抗 2.2kΩ 2個

## カップリング
<img width="50%" alt="カップラ.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/b1bc8136-ffd0-cd82-4ed8-cae2d1e18b49.jpeg">

千石電商にて購入
[4Pカップリング](https://www.sengoku.co.jp/mod/sgk_cart/detail.php?code=EEHD-5F3D)

ロータリーエンコーダとギア・ボックスのシャフトを接続するのに使用します。
後述しますが、接続の際にはシャフト側に少しだけ工夫が必要になります。

## スペーサーなど
<img width="50%" alt="スペーサー.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/74da35c7-e1c2-ada4-1692-c80934c3b480.jpeg">

秋葉原の専門店で購入。
基板とシャーシを固定するのに使用します。


## 電子工作道具
普段から電子工作をしている人には必要ないと思いますが、以下の道具を使用します。マルツパーツやAmazonなどの通販で全て揃います。

- ハンダごて
- ハンダ
- ニッパー
- 導線
- ワイヤーストリッパー
- 精密圧着ペンチ
- ピンセット

# 傾斜計の作成

分割ロングピンソケットをマイコンボードにつけます。
<img width="50%" alt="傾斜計1.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/32747ab9-5425-d95a-3b51-a7ded0650a6a.jpeg">

マイコンボードとセンサ用のICソケット6P を基板にハンダ付けして、配線をします。
<img width="50%" alt="傾斜計5.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/fb755ec9-0bd7-d27b-f2af-22c614eb9c67.jpeg">
<img width="50%" alt="傾斜計4.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/2f0527e9-5cc7-2e8f-77c7-07b02c9839be.jpeg">


センサにピンをハンダ付け
<img width="30%" alt="傾斜計2.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/6000e8fb-9595-1fa0-7c3d-a51e89a42091.jpeg">

内蔵プルアップ抵抗を有効にするためにJP4とJP5を接続。
3.3Vを有効にするためにJP7を接続。トランジスタ技術にはJP7とJP8を接続すると書いてあったのですが、データシートには、その組み合わせは存在しませんでした。

それと、ピンが邪魔で作業しづらかったので、この作業はピンをつける前にすべきでした。
<img width="30%" alt="傾斜計3.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/1805b13d-37f4-2e44-057b-5c80f2c1384f.jpeg">

# 傾斜計の動作確認

## 開発環境構築
mbedアカウントでオンラインの開発環境を利用します。
付録DVDの付記資料に従って進めます。

mbed 内の NUCLEO-F401RE のページ にアクセスし、「</> Open Mbed Compiler」をクリックします。
![mbed1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/920c9477-8261-2839-ca13-aea57b5d99d2.png)

プラットフォーム名の「NUCLEO-F401RE」を選択し、新規からテンプレートを元にプロジェクト作成します。
私は試験毎に、新しいプロジェクトを作成しました。
![mbed2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/a6ff033f-450d-6d2f-91b1-70a3f25854e5.png)

[ST-Link driver](https://www.st.com/en/development-tools/stsw-link009.html)をインストールします。
![STSW-LINK009b.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/521a4cd7-70bb-3208-a1d1-b213338399f0.png)

## UART通信試験
付録DVDを参照して、UART通信ができることを確認します。
<img width="80%" alt="UART試験1.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/99050048-cc8f-b2d1-7043-67a4667f0b05.png">

## I2C Read/Write試験
付録DVDを参照して、センサとI2C通信ができることを確認します。
<img width="80%" alt="i2c_read_write試験.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/9a77236e-affe-6b21-7fff-90013d25cecf.png">

センサにchip ID要求命令を書き込み、応答を読み出せるか確認しています。

## 加速度センサ試験
付録DVDを参照して、加速度センサが正常に動作しているかを確認します。
<img width="70%" alt="加速度センサ試験.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/0acf1d81-def8-f628-f89c-f676d645dac3.png">

## ジャイロ試験
付録DVDを参照して、ジャイロが正常に動作しているかを確認します。
<img width="70%" alt="ジャイロ試験1.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/f3ec1c4b-263c-bec0-8116-e3ec4c0a85b1.png">

## センサ2000回測定
加速度センサから角度を2000回測定し、グラフ化します。
![加速度計のヒストグラム.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/f6c2cb2e-98af-8562-7c1c-5f3ea1eefca3.png)

ヒストグラムは、正規分布の形になっています。
「測定雑音が正規分布に従っている」ことが分かったので、カルマンフィルタを適用する根拠になります。

データはTeraTermのログ機能を利用してテキストに保存しました。
計算とグラフ化はPythonを使用しています。

同様に、静止状態のジャイロセンサを2000回測定してばらつき具合を確認します。
![ジャイロのヒストグラム.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/e1446a89-a3f7-a7bb-fe29-fce2e3eaf4b5.png)

これも正規分布になっています。

## カルマンフィルタ試験
雑誌に掲載されているカルマンフィルタを実装します。
USB接続部を上にして、なるべく角度を固定した状態で、前後左右上下に振ります。
その計測結果を見ると、正規分布になっており、加速度の激しい揺れによる角度変化が起きていません。
フィルタが正しく機能していることの証左であります。
![傾斜計試験結果.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/17870d5b-6781-77b6-73a4-308e54bd036a.png)



# 倒立振子の作成
センサとカルマンフィルタが正常に機能していることが分かったので、次はいよいよ倒立振子を作成します。

## ギア・ボックスの組み立て
基本的に付属の手順書通りに組み立てます。
組み立て時の注意点は以下の通りです。

- ギア比は64.8:1を選ぶ
- シャフトは2本あるが、両端に穴が開いているタイプを選ぶ
- シャフトの通し穴は２つあるが、真ん中の穴に通す
- シャフトの固定位置を、内側が穴のギリギリになるようにする
<img width="50%" alt="ギアボックス_シャーシ.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/0a4e579b-6ee9-fbea-00d7-172975cadd99.jpeg">

タイヤを取り付けます。
<img width="50%" alt="ギアボックス_シャーシ2.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/4220f946-dbbf-8798-c96c-1c9bedf457fa.jpeg">
<img width="50%" alt="ギアボックス1.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/66be130f-ee5e-27f4-a55c-f15b7f0a9156.jpeg">
<img width="50%" alt="ギアボックス_シャーシ3.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/2390346f-7271-06d9-0b32-9f3f44aa2e5e.jpeg">
<img width="50%" alt="ギアボックス_シャーシ4.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/4464dc92-00b6-4e44-1ed2-00ff4d119eba.jpeg">


## ロータリーエンコーダとギア・ボックスの接続
シャフトとロータリーエンコーダを接続するのにはカップリングを使用します。
シャフトの径は細すぎて固定ネジが届かないので、ギアボックス付属のナットを使って径を太くします。
<img width="50%" alt="ギアボックス_シャーシ5.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/d61e0382-93e7-c35d-029a-278950898657.jpeg">

カップリングを仲介して、シャフトとロータリーエンコーダを接続します。
<img width="80%" alt="ギアボックス_シャーシ6.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/977296f8-8518-343d-e601-d26cfe6be3dc.jpeg">

スペースの固定には適当にスポンジを挟みました。
<img width="50%" alt="ギアボックス_シャーシ7.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/ca50d19c-d3a4-4c45-295a-c7871de3e506.jpeg">

シャーシの下から7番目の穴の高さにギアボックスを取り付けて、両端2ヶ所をネジとナットで固定します。
<img width="90%" alt="ギアボックス2.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/dba4e352-f384-e7e6-e826-0eb38445bf5e.jpeg">

## 配線
雑誌の回路図通りに配線します。

<img width="100%" alt="配線1.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/98f7d27a-162c-1b63-a984-855d3ffb3137.jpeg">


<img width="100%" alt="配線2.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/a30d4544-2357-c820-5f62-8851c2abbc5f.jpeg">

写真はまだグランドとモーターの配線はしていない時のものです。

配線は単純な作業ですが、5時間ほどかかりました。
キットを購入すれば、専用基板が用意されているので、この手間が省けます。
配線をしながら回路構成を何度も確認するので勉強になりますが、手配線は大変で、ミスによる動作不良も起きやすくなるので、カルマンフィルタの学習に専念したい方はキットを購入する価値があると思います。


## 組み立て
![組み立て.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/150620/26f5c803-a0a4-604a-feeb-6128d6e3d351.jpeg)

ギアボックス、ロータリーエンコーダ、基板、電池ボックスを組み立てます。
基板をシャーシに固定するのは、ギアボックス付属のネジを使用しました。
電池ボックスは、取り付け用のネジ頭が平たいものがなく、臨時でテープで貼り付けて固定しました。後でキチンと固定します。

モーターはコネクタで接続するのではなく、ハンダで直付けしました。
モーターは青い線をモータードライバのOUT1に、赤い線をOUT2に接続しました。
モーターには極性があるので、反対に接続すると、サンプルプログラムの出力も反対にしないといけないはずです。そうしないと意図しているのと逆方向に回転してしまいます。

ロータリーエンコーダのコネクタの長さが足りず、裏で導線で接ぎ木のように伸ばしています。
また、ノイズ対策に、モーターとセンサに0.1uFを追加してあります。


## 動作確認
作成した倒立振子にサンプルプログラムInverted_Pendulum_Kalman.cppを書き込んで動作確認します。

電池は充電式のニッケル電池1.2Vを使用しています。アルカリ電池を使用すると1.5Vなので4本で6Vになってしまいます。

直立させた状態で電源スイッチを入れ、黄色LEDが点灯してから消灯するまで固定します。
（調整不足のせいだと思いますが、やや手前に倒しておくと、その後の動作が安定しました。）

私の場合、以下のように、不安定ながらも倒れずに動作しました。
モーターやセンサーまわりに配線不良のないことが分かりました。
ただ、赤色LEDが点灯しないので、何が原因か調査する予定です。

<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">トランジスタ技術2019年7月号の倒立振子が完成しました。<br>以下の動画のように、前後にフラフラとしながらもバランスを保って倒れずにいます。 <a href="https://t.co/nnkOWrBpTN">pic.twitter.com/nnkOWrBpTN</a></p>&mdash; 松井 (@RB_79K_BALL) <a href="https://twitter.com/RB_79K_BALL/status/1146767437297156097?ref_src=twsrc%5Etfw">2019年7月4日</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

# 今後の予定
倒立振子を作成して動作するところまでは確認しました。
次は実際に自分で倒立振子のシミュレーションをして、パラメーターを求める予定です。
力学モデルも自分の作成した倒立振子に合わせて計算しなおしたいです。

また、雑誌後半の理論面の解説記事を実際に計算しながら読んでいきたいと思います。

新しく進捗や発見があれば、また別途記事にしたく思います。