ラズパイ日記...25日目...ちょっとPi zeroでmpu6050

このところ、ESP32+micropython環境でずーと頑張ってましたが、micropythonはモジュールに制限があり、大きな配列等は扱えません。

加速度センサー値をMadgwickフィルターを通してみたいのですが、micropythonのモジュールは見つかりませんでした。Pythonでは沢山あるので、一時的にPi zeroでセンサー調べを行いたいと思います。

半年振りにPi zeroを引っ張り出してきました。

センサーとも接続し、値を読み出してみることに。

接続はPi zeroとI2Cなので、3.3V, GND, SCL, SDAの4線を接続すればOKです。
あれ、Linuxではコマンドなんだったけ？と完全に忘れてます。
あと、
%sudo apt-get update
%sudp apt-get upgrade
したら、 30分ほど終りませんでした。

ESP32からPi zeroに環境変えるだけで1日費やしました。
Linuxもコマンド叩いてないと、結構忘れてます。
Pi zeroをシリアル接続する方法。
https://qiita.com/mt08/items/84d3d3053bee92609c2d

このページを参考に、センサーの値を読み出して、角度表示させるまでは出来ました。
http://www.widesnow.com/entry/2015/09/06/211055

 あとは、この値をMadgwickフィルターに渡して、フィルターなしの場合をグラフで比較したいと思います。Linuxでグラフ表示はどうするのか。しばらくは、Pi zeroでがんばってみたいと思います。

ラズパイ日記...24日目...倒立反省＋センサー見直し

倒立振子2ヶ月ほど頑張りましたが、いまだ倒立せずです。
いろいろと振り返って、改善ポイントをまとめると以下のとおりです。

本体

• 重心は車軸の上に。出来るだけ重心上にものを集めて、重量物なモーターや電池は軸下に配置し、モーターが出来るだけ軽い力で制御できるようにする 

センサー

• 生データのドリフトやばらつきは、しっかりフィルターで除去する
• あまりフィルターかけすぎると、遅延が発生するので精度と応答速度のバランスが重要

モーター

• DCモーターは、電圧をかけてからトルクが出るまでに時間を要する（200-300ms）
• 電池の状態は、常に気にしないと充電したてと放電してからでは制御パラメータが異なる

制御プログラム 

• 単一ループでセンサー読み取り→モーター駆動するとモーター駆動間の待ち時間に処理が止まる。センサー読み取りとフィルター処理部分とモーター駆動処理は別のタスクにした方がうまくいくかも（未確認）

と、これまでの反省点です。半日で倒立というblog記事ありますが「倒立は一日にして立たず」が実感です。

まずは、センサーの見直しというか、他のセンサーはどんな動きなのかというのを調べるために2つ新たに購入しました。

左はKXSD9-2050（秋月で720円）、右はMPU-6050（Amazonで320円）とAmazonの方がお安いですが、カメラやビデオの手ブレ補正に使われてるらしく数が出ているものだと思います。

まずは端子を半田付けします。

接続はI2CなのでVCC,GNDとSDA,SCLの4線です。レギュレーター内臓なので、VCCは５V,3.3VいずれでもOKです。
まずはMPU6050から動作確認のためESP32と接続します。

 

micropythonのライブラリを探して、出力されているところまでは確認しました。
あとは、師匠に教えてもらったMadgwickフィルターを調べて、LPFと動きの違いを検証してみたいと思います。

ラズパイ日記...23日目...重心を下げて軸上に

本体の低重心化です。基板等はゴムバンドで止めているだけなので、ぱっと外せます。

付け替えるのが面倒なのはギヤボックスです。ユニバーサル基板と微妙に大きさがネジ穴の位置が違うため、切欠きを新たに。

 ギヤボックスと電池を床下において、軸上に重心センターがくるべく調整します。
重心を合わせるために計量です。
ギヤボックスは、82g。

 

電池ケースは、90g。

 

 差は8gです。これは後で微調整。

あとは、床上に配線の取り回しを考えて基板を配置します。

DCモーターへの配線が短かったので、長いものに交換します。

 

 動作確認して再度パラメータ探しの旅です。高重心の時に比べると、倒立しそうな感じですが、なかなかじっとしません。

師匠から重心とDCモーターの残電池容量の確認、モーターコントロールをタイマー割り込みをやってみるとよいとのアドバイスをもらいました。
電池は再度充電、重心はクリップで微調整しほぼ車輪軸上になりました。

この1週間だいぶ戯れましたが、今日のところまでで「倒立」せずです。（とほほ）

しかし、micropythonの勉強や、9軸センサーの補正方法、DCモーターの特性等いろいろと勉強になりました。頭で理解したつもりでも、実際はいろいろとうまくいきません。
あと、タイマー割り込みはまだ勉強中です。これも取り入れていきたいと思います。

残り少ないGWですが、最後に「倒立」するのか？

ラズパイ日記...22日目...倒立調整中→重心を低く

倒立振子の母体は完成し、センサー出力とDCモーターは連動することまでは確認できました。ただ、このままでは倒立しません。センサーとモーター出力を調整する必要があります。

とりあえず、センサー出力を可視化して眺めてみます。

ジャイロの出力が急峻なのでLPFで滑らかに、加速度は出力をそのまま使うのではなく、角度情報に変換して活用します。

加速度センサーから角度への変換はこちらを参考に
https://garchiving.com/angle-from-acceleration/

micropythonでCtrl-Cを掴んで、終了処理をする方法については以下を。これをいれないとモーターが動きっぱなしになります。
https://qiita.com/keisuke-nakata/items/e77b123d75d4b52aac77

当初のセンサー出力です。センサー出力そのままです。

 青線が加速度、赤線がジャイロの出力です。傾きとジャイロの方向は合っています。
このジャイロ出力にLPFを、加速度はatan2(dx,dy)を計算すると角度になるということで、その結果を入れたのがこちら。

ジャイロ出力がマイルドになり、加速度も角度に変換したので、滑らかに角度を表してます。
これで、加速度とジャイロの係数、LPFの係数、モーターの駆動時間等を変化させながら、最適値を探りましたが、最適値にはたどり着かずです。

これで最適値を探す旅もつらい感じなので、まずはボディーの低重心化をして再挑戦したいと思います。
ざくっとは、こんな感じです。結構重心は下がりました。
BEFORE（高重心版）

AFTER（低重心版）

ラズパイ日記...21日目...倒立振子立ち上がるのか

一通りの配線確認を行い、それなりにセンサーの値を読み取り、モーターも正転、逆転できるところまではきました。

本体を組み立てていきます。

まずは、ギヤボックス。ネジ位置が板とあわないので、カッターでちょっと切り欠きます。後ろにプラスチックの板でユニバーサルプレートをネジで挟んで両側止めます。ここがグラグラすると全体に波及するので、プラスドライバーとラジオペンチでしっかりとめます。

真ん中と上部に基板を引っ掛けるL字金具を取り付けます。基板は幅広ゴムで止めます。

微妙に配線がきついですが、何とかなりました。
 全体はこんな感じです。

上部は、ESP32と9軸センサーです。

 

DCモーターコントローラー。LEDは、PWM出力を明るさで確認できます。

ギヤボックス

背面はモーター駆動用の電池をゴムで取り付けます。

なんとか、全体的な形になりました。
再度配線を確認して、これまで個別のプログラムを 引っ付けて、センサーとモーターの動きを制御したいと思います。

さて、GW中に倒立するのか。

ラズパイ日記...20日目..DCモータコントロール

倒立振子実現のために、事前確認です。

DCモーターコントローラは、東芝製のTA7921Pを使用してます。
こちらに詳しく説明とサンプルがあります。
https://www.marutsu.co.jp/contents/shop/marutsu/mame/81.html

回転方向の制御に2線、回転速度の制御に1線を使用します。
ESP32側は、回転方向制御に1,0をそれぞれ出力してあげれば問題ありません。
回転制御は、アナログ出力はないのでPWM出力をアナログ出力とみなして制御します。
周波数は、500Hzから1kHzがいいようです。
デューティー比を0(0%)～1023(100%)まで変化させると擬似的にアナログ出力となります。
デューティー比は、0→1→10→100→500→1000と変化させた時のモータの動きです。

デューティ比0,1,10ではモーターは動きません。 PWM信号にLEDをつけてます。段々明るくなっている状況がわかると思います。

もう1個のモーターも配線を半田付けして、モーターコントローラと配線します。

反対側を見ながらコピーすればいいので簡単です。プログラムを反対側分も作成し、いざテスト。
うーん、新しく配線したモーターは、うんともすんとも動きません。反対側は問題なく回ってます。
再度、配線を見直しても間違ってません。
配線触ってると、モーター動き出しました。原因はここ。

モーターコントローラーのVCC線の剥き方が気持ち短くて、ブレッドボードと微妙に接触不良です。長めに剥いて配線しなおすと、左右同じように動くようになりました。

これで、左右のモータが動くようになりました。将来的な拡張を考えて左右個別にコントロール出来るようにしてあります。

あとは、これまでのセンサー読み出し、モーター駆動等の個別コードをまとめて、倒立できるようにプログラムの作成です。
基板に取り付けて、ようやく倒立実験です。GW中は楽しめそうです。

ラズパイ日記...19日目...9軸センサ(その3)

倒立振子のためにESP32をもう1個買いました。

あわせて片側6穴のブレッドボードも購入です。これで両側に配線が可能です。

左が新規購入した6穴、右がこれまで使ってた5穴です。5穴だとESP32の横幅が収まらずどちらかサイドのピンが使えなくなります。

 あとは、ArduinoIDEの新しいのは、シリアルデータをグラフにしてくれる機能があるようで、1.6.4から1.8.9にアップデートしました。

 

シリアルプロット試してみました。シリアルモニターからコマンド打って、シリアルにCSVデータが出力する状態でシリアルモニターをクローズして、シリアルプロットを立ち上げます。シリアルモニタとシリアルプロットは同時オープンできません。データ毎に色を変えてグラフにしてくれました。
加速度のx,y,z、ジャイロのx,y,xの6軸の状況です。センサをｘ方向とかｙ方向とか動かすと、それなりに反映されている感じです。

新規購入したESP32にもmicropythonを入れて、センサーを移し変えて配線をし直します。pythonコードもそのままコピーしてやれやれと思ったら、 センサ情報が出力されず、謎のエラーがでます。いろいろと確認したらI2CのSDA、SCLが逆になってました。
確認したつもりが、見直すと誤配線。配線は、少し間を置いて気分をリフレッシュしながら何度も要確認です。

モーター回路にも接続してモーター回ることを確認します。こちらは問題ありませんでした。

ということで、9軸センサとDCモータードライバーの配線は新しいESP32へ引越し終わりました。

センサーは、1方向の加速度とジャイロしか使わないので、x軸のみ加速度とジャイロ出力をグラフに出力して見てみます。

傾きが青線、加速度が赤線です。傾きにあわせて青線が動きます。早く動かすと赤線が上昇し、ゆっくり動かすと加速度は動きません。赤線は、微妙にオフセットされているので、これも補正しないとですね。
あとは、動かさなくても値が微妙に変化するので、感度を調整するか、制御ソフトで取捨選択するか考えないといけません。