前回の記事「MPU6050 + Raspberry Pi Pico(Arduino) -> PCで3Dのキューブを回転表示」の続きです。

前回はジャイロ・加速度センサー(MPU6050)の値を読み込んでPC上で3Dのキューブを回転表示しましたが、
今回はサーボ2つを使用して、「常に水平になる台」を作成してみました。

サーボ2台を直交させて接続しますが、お手軽にラズパイカメラ用2軸マウント(A838)を使用しました。

このカメラマウントは、サーボに定番の小型サーボSG-90を使用するようになっています。
(今回はSG-90の互換サーボを使用しました)

ラズパイカメラ用2軸マウント(A838)の組み立て方法は以下を参考にさせていただきました。
https://python-academia.com/raspberrypi-camera-mount/

上の写真のように接続しました。

  Pico               MPU6050
  Pin6 GP4        -> SDA
  Pin7 GP5        -> SCL
  Pin36 3V3(OUT)  -> VCC
  Pin3 GND        -> GND

  Pin4 GP2        -> サーボ1
  Pin5 GP3        -> サーボ2

プログラムはMadgwickAHRSで推定した角度でサーボを動かしています。

Raspberry Pi Pico側はArduino開発環境を使用しました。
Arduino開発環境にはearlephilhower版を使用しました。

以下のライブラリを使用しています。
GitHubからZIP形式でダウンロードして、Arduino IDEの「スケッチ」->「ライブラリをインクルード」->「zip形式のライブラリをインクルード」でインストールします。

〇角速度・加速度から角度を推定 -> MadgwickAHRSフィルタ
https://github.com/arduino-libraries/MadgwickAHRS

〇タイマー割込み
https://github.com/khoih-prog/RPI_PICO_TimerInterrupt

ソース(Arduino開発環境用)はこちらに置いています。
https://github.com/ponta1/HorizontalTable

ソース(Arduino開発環境用)はこちらに置いています。
https://github.com/ponta1/HorizontalTable

コップの水がこぼれない様子
※サーボが小型のためパワーがなくてダイソーのおちょこぐらいしかのせられませんでした

Tweet

前回の記事「Madgwickライブラリの収束を速くする 【Arduino/MPU6050】」の続きです。

角速度(ジャイロ)・加速度センサーMPU6050の角度をPC上で表示するプログラムを作ってみました。

Raspberry Pi Pico(Arduino開発環境)でMPU6050から読み取った角速度・加速度からMadgwickで角度を推定しPCへ送り、PC上で受け取った角度で3Dのキューブを回転させ表示します。
今回はRaspberry Pi Picoを使用しましたが、Arduino UNOなどでも接続ピンが変わるのとタイマーのところを変更すれば行けると思います。

プログラムはGitHubで公開しています。

■Raspberry Pi Pico側
PicoとMPU6050は上の写真のように接続しました。

  Pico               MPU6050
  Pin6 GP4        -> SDA
  Pin7 GP5        -> SCL
  Pin36 3V3(OUT)  -> VCC
  Pin3 GND        -> GND

Raspberry Pi Pico側はArduino開発環境を使用しました。
Arduino開発環境にはearlephilhower版を使用しました。

Raspberry Pi Pico側のプログラム(Arduino)
MPU6050/MPU6050.ino

以下のライブラリを使用しています。
GitHubからZIP形式でダウンロードして、Arduino IDEの「スケッチ」->「ライブラリをインクルード」->「zip形式のライブラリをインクルード」でインストールします。

〇角速度・加速度から角度を推定 -> MadgwickAHRSフィルタ
https://github.com/arduino-libraries/MadgwickAHRS

〇タイマー割込み
https://github.com/khoih-prog/RPI_PICO_TimerInterrupt

■PC側
MPU6050.py

Pythonで作成しています。
COM_PORT変数は適切なポートに書き換えてください。

以下のライブラリを使用しています。
〇pyserial
PCとPicoのシリアル通信のためにpyserialを使用しています。
以下のコマンドでインストールしてください。

pip install pyserial

〇Panda3D
3D表示のためにPanda3Dを使用しています。
以下のコマンドでインストールしてください。

pip install Panda3D

Tweet

ArduinoでMPU6050などのジャイロを使うとき、Madgwickライブラリを使うと簡単に角度の推定ができますが、デフォルトの状態だと収束に少し時間がかかるようです。
多少ノイズが大きくなっても速く追従させたかったので、収束速度=ゲインを調整できるようにしてみました。

Madgwickライブラリ: https://github.com/arduino-libraries/MadgwickAHRS

Madgwickライブラリの使い方はここでは述べませんので以下を参照してください。

ArduinoのMadgwickライブラリの使い方
https://garchiving.com/how-to-madgwick-library-of-arduino/

【Arduino 】MPU-6050を使って姿勢角を算出　（MadgwickFilterを使用）
https://shizenkarasuzon.hatenablog.com/entry/2019/02/16/181342

Madgwickライブラリは1.2.0 (2022/12/3時点の最新版)を対象とします。

Madgwickライブラリのゲイン変更

Madgwickライブラリではゲインが0.1で固定されていますので、これを変更できるようにメソッドを追加してみます。

Madgwickライブラリをインストールすると、手元の環境では以下のディレクトリにインストールされていました。
C:\Users\xxxx\Documents\Arduino\libraries\MadgwickAHRS-master\src
(※xxxxはユーザー名)

MadgwickAHRS.h にsetGain()メソッドを追加します。 (begin()メソッドの下の行)

....
public:
    Madgwick(void);
    void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
    void setGain(float gain) { beta = gain; }	// <- この行を追加
    void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
....

begin()を呼んだ後に、以下のようにsetGain(1.0)すると、ゲインを設定できます。

  MadgwickFilter.begin(100); // 100Hz
  MadgwickFilter.setGain(1.0);

デフォルトは0.1です。
1.0にすると、収束が速くなります。ただし、ノイズが大きくなりますので、用途に合わせて調整してください。

Tweet