【M5Atom入門】内蔵IMUとservoのPID制御で倒立振り子を立ててみる♪

前回、ESP32とm5stickcを利用して倒立振り子をPS3コントローラーで外部制御して立てることができた。
今回は、一応たぶん世界一小さなマイコンであるm5atomを利用して同じことができかどうか挑戦した記録です。
m5atomもウワンはごく最近知ったのだが、どうやら昨年の１月に販売開始して、国内もスイッチサイエンスで2020年4月に販売開始されている。
なので、もう十分こなれたものかと思ったが、ググってもあまり見つけられなかった。
特に、複数servoは結論から言うと、m5stickcと同じように、ledcWriteで複数制御できたが、情報は少ない。
###やったこと
・はじめてのatom
・IMUを動かす
・PCA9685で複数サーボを制御
・pwmledで複数サーボを制御
・PS3コントローラーを設定してbluetoothを利用する
・倒立振り子を立てる
###・はじめてのatom
m5atomあまりに小さいので本当に動くのかという疑問。

まず、今回利用するmatrixのよく見る機能と配置を示した図
※右下にesp32-picoとボード名、サイズ、
　左下に利用できる言語；Arduino, UI-FLOW. MicroPYthon
　Button（G39）の位置（実際画面が押せます）、25X25のLED（G27）
　真ん中に取り付け用M2(ネジ)の穴、外部ピン二ヶ所（下の表参照）
　GroovのPH2.0-4P(コネクタの規格)、USB-Cコネクタ
　Resetボタン、IR-LEDの位置

![m5atom_hp.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/77937bfc-ebda-a4b4-990f-7f33477f3d35.png)


pin配置は以下の背面図が分かり易いです。
![m5atom.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/2576e5c5-f06c-e634-b69f-acbd2ebb72d3.jpeg)
予約されているPinは以下のとおりです。
（[https://github.com/m5stack/M5Atom](https://github.com/m5stack/M5Atom)より）

|用途|GPIOPin番号|
|:-:|:-:|
|Neo|GPIO27|
|Btn|GPIO39|
|CLK(MPU6886)|GPIO21|
|SDA(MPU6886)|GPIO25|
|IR|GPIO12|
|Btn|GPIO39|
またPH2.0で結線できる便利なGroveのPin配置は以下の通りです。

|名称|GPIO|GPIO|||
|:-:|:-:|:-:|:-:|:-:|
|ESP32 chip|GPIO32|GPIO26|5V|GND|
|PH2.0 interface|SCL|SDA|5V|GND|
※SCL, SDAは外部I2C機器の制御ができます

その他、以下のPinが使えます。

|GPIOPin番号|用途|
|:-:|:-:|
|3.3V|3.3v出力|
|G32|GPIO32|
|G19|GPIO19|
|G23|GPIO23|
|G33|GPIO33|
|RST|RST|
|G21|GPIO21|
|G25|GPIO25|
|5v|入力・出力|
|G|GND|
※基本はESP32マイコンなので、それぞれのpinの用途はプログラム次第でいろいろな用途として使えそうです.

一方、プログラムは、以下の感じです。
一応、M5Stackの一員としてm5atomの項目にサンプルが掲載されています。
これらのサンプルは、m5stickcとほぼ同じように利用できます。
つまり、ArduinoIDEで、基本はボードのESP32で、その中のESP32　pico　Kitです。
そして、利用できるスケッチは「ライブラリを管理」で、いつものようにm5atomと入力して検索すると、インストールが出てきます。
これで、スイッチの中に「ファイルースイッチ例－M5Atom-」の配下に３つのカテゴリで使えるようになります。
[M5Atom/examples/](https://github.com/m5stack/M5Atom/tree/master/examples)のATOM＿BASE、Basics, Echoです。このうち、初心者ウワンだとBasicsしか見てすぐわかるのがないので、この中で簡単そうなものをまず動かしてみました。
####LEDDisplay
これはAtomの特徴になっている、前面のLEDを全部使ってATOMという文字をスクロールするアプリです。
しかし、掲載されているものは、一度動いて止まってしまいます。
そこで、`update_anim()`関数を作成してloop()から呼ぶように変更しました。
これで`if (M5.Btn.wasPressed())`とこれもAtomの特徴である前面のボタンを押下すると、ATOMの文字が流れるように変更できました。
※**このアプリで前面のLEDの制御とボタンの使い方を理解できました**
 あと、推測ですが、`bool IMU6886Flag = false;`はこういう小さいなマイコンでは重要でIMUを使わないというフラグで、これでIMUが使っているI2C利用のPin（G21,G25）を自前で使えるんじゃないかと思います。また、IMUが動かないというのは計算コストからも優位なんでしょうかね。
【参考】
①[M5Atom/examples/Basics/LEDDisplay/](https://github.com/m5stack/M5Atom/tree/master/examples/Basics/LEDDisplay)
※以下のコードの他に、image.c（ATOMのLEDを発光させるためのデータ）が必要です.
　これはスケッチ例から起動すれば自動的に読み込まれます。

```LEDDisplay_ex.ino.c
#include "M5Atom.h"
/* this Example only for M5Atom Matrix */
extern const unsigned char AtomImageData[375 + 2];
bool IMU6886Flag = false;
void setup()
{
    M5.begin(true, false, true);
    delay(50);
    M5.dis.animation((uint8_t *)AtomImageData, 200, LED_DisPlay::kMoveLeft, 18);
}

void loop()
{
    delay(50);
    if (M5.Btn.wasPressed())
    {
      update_anim();
    }
    M5.update();
}

void update_anim(){
  M5.dis.animation((uint8_t *)AtomImageData, 200, LED_DisPlay::kMoveLeft, 18);
}
```
###・IMUを動かす
今度は、内臓MPU6886というIMU（慣性計測装置（IMU：Inertial Measurement Unit））を動かします。
[前回も、m5stickcで同じ内臓IMUを利用](https://qiita.com/MuAuan/items/afbc07af06209d1a563c)しました。
しかし、コードは以下のとおり、少し異なります。
呼び出しが、m5stickcでは`M5.MPU6886.Init();`のようになっていましたが、m5atomでは`M5.IMU.Init();`となっています。
また、`M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);`の関数も`M5.IMU.getAttitude(&pitch, &roll);`を利用するようです。
※さらなる参考として以下をあげておきます.
【参考】
[カルマン・フィルタで ATOM Matrix 傾斜計 ーリアクションホイールへの道3ー](https://homemadegarbage.com/reactionwheel03)

[M5Atom/examples/Basics/MPU6886/](https://github.com/m5stack/M5Atom/tree/master/examples/Basics/MPU6886)

```mpu6886_ex.c
#include "M5Atom.h"

double roll, pitch, yaw;
float accX = 0;
float accY = 0;
float accZ = 0;

float gyroX = 0;
float gyroY = 0;
float gyroZ = 0;

float temp = 0;
float r_rand = 180 / PI;

void setup() {
  M5.begin(true, true, true);
  M5.IMU.Init();
}

void loop() {
  // put your main code here, to run repeatedly:
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  M5.IMU.getTempData(&temp);
  M5.IMU.getAttitude(&pitch, &roll);
  double arc = atan2(pitch, roll) * r_rand + 180;
  double val = sqrt(pitch * pitch + roll * roll);
  Serial.printf("p%.2f,r%.2f,a%.2f,v%.2f\n", pitch, roll, arc, val);
  Serial.printf("gx%.2f,gy%.2f,gz%.2f,%.2f\n", gyroX,gyroY,gyroZ);
  Serial.printf("ax%.2f,ay%.2f,az%.2f,%.2f\n", accX,accY,accZ);
  Serial.printf("Temperature : %.2f C", temp);
  delay(100);
}
```
###・PCA9685で複数サーボを制御
[m5stickcと同様](https://qiita.com/MuAuan/items/afbc07af06209d1a563c)に、Groveを使ってPCA9685を利用して複数サーボを制御します。
m5atomでも宣言以外同様で動きました。
また、`Adafruit-PWM-Servo-Driver-Library`は、m5stickcで利用したままで使えました。

```pca9685_atom.ino.c
#include <M5Atom.h>
#include <Wire.h> 
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN 102 
     // 下で設定するPWM設定周波数を4096で分割したタイミングで制御する最小パルス幅　：4096/20×0.5＝102.4　（0.5ms：SG90、0°の時のパルス幅）
#define SERVOMAX 492 
     // 下で設定するPWM設定周波数を4096で分割したタイミングで制御する最大パルス幅　：4096/20×2.4＝491.5　（2.4ms：SG90、180°の時のパルス幅）
int sk =0;

void setup() {
 M5.begin(true, false, true);
 Wire.begin(26,32); //i2cのpin宣言が重要 SDA, SCL
 pwm.begin();
 pwm.setPWMFreq(50); // SG90は 50 Hz で動く（PWM周波数設定）
 
}

void loop() {
  servo_write(0, 1*sk%70+80);
  servo_write(15, 1*sk%70+80);
  delay(50);

  sk += 1;
  M5.update();
}

void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
  ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅（150～500）に変換
  pwm.setPWM(ch, 0, ang);
}
```
###・pwmledで複数サーボを制御
このコードもほぼm5stickcと同様です。
Pin番号をGroveの26と32を出力用として利用しています。
※注意；Servoはここではsg90利用して動くのを確認していますが、関数等はfs90r前提で設定しています

```pwmled_atom.ino.c
//#include <M5StickC.h>
#include <M5Atom.h>

int PIN0 = 26;
int PIN1 = 32;
int PWMCH = 0;
int PWMCH1 = 1;

void setup() {
  M5.begin();

  pinMode(PIN0, OUTPUT);
  pinMode(PIN1, OUTPUT);
  ledcSetup(PWMCH, 50, 12); //8bit
  ledcAttachPin(PIN0, PWMCH);
  ledcSetup(PWMCH1, 50, 12); //8bit
  ledcAttachPin(PIN1, PWMCH1);
}

void loop() {
  servo_write(PWMCH, -60);    //   0.7msec(-90)
  servo_write(PWMCH1, -60);    //   0.5msec(-90)
  delay(1000);
  servo_write(PWMCH, 0);  // 246 = 1.5msec(0)
  servo_write(PWMCH1, 0);    //   0.5msec(-90)
  delay(1000);
  servo_write(PWMCH, 60);  // 491 = 2.3msec(90)
  servo_write(PWMCH1, 60);    //   0.5msec(-90)
  delay(1000);
}

void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
  ang = map(ang, -90, 90, int(0.7*4096/20), int(2.3*4096/20)); //角度（-90～90）をPWMのパルス幅（700～2300）に変換
  ledcWrite(ch, ang);
}
```
###・PS3コントローラーを設定してbluetoothを利用する
これも無事に同様に利用出来ました。
まず、前回同様、m5atomのmac addressを以下のように取得します。

```Ps3Address.ino.c
#include <Ps3Controller.h>

void setup()
{
    Serial.begin(115200);
    Ps3.begin();

    String address = Ps3.getAddress();

    Serial.print("The ESP32's Bluetooth MAC address is: ");
    Serial.println(address);
}

void loop()
{
    String address = Ps3.getAddress();

    Serial.print("The ESP32's Bluetooth MAC address is: ");
    Serial.println(address);
    #The ESP32's Bluetooth MAC address is: 94:b9:7e:92:4a:da
}
```
###・倒立振り子を立てる
いよいよ倒立振り子を立てるプログラムの完成です。
コードは、やはりm5stickcのコードとほぼ同様です。
今度は、配線のし易さからservo制御のpinを33、23に変更しています。
また、m5atomのIMUは水平にして利用しました。
これは、IMUを立てるとrollを回転していったとき、丁度鉛直方向を向いたとき44から―44に飛びが出てしまったことから、飛びが無い0度近傍で実施しました。
※この44辺りの数字が90で無いのが気に入りませんが、指定していませんが、どうも測定条件かなと推測していますが、確認できていません。

いずれにしても、先ほど取得したmac addressを[前回Windowsにインストールした書込みツール](https://qiita.com/MuAuan/items/6f8f5d0587798b3c6d8b)を利用してPS3コントローラーに書き込み、以下のプログラムを動かしてみます。
電源は、当初はUSB-Cでモニター見ながら実施しますが、途中からGrove5vとGND供給で動かすことが出来ました。m5atomも内臓電池を持っていませんが、給電なくても再度給電すると動くことからUSB-Cを外してから電源繋ぐまでがスムースに実施できました。
※電源変更したとき、PS3のPS3ボタンで接続してパラメータ調整が必要です

```touritu_atom.ino.c
//#include <M5StickC.h>
#include <M5Atom.h>

int PIN0 = 33;
int PIN1 = 23;
int PWMCH = 0;
int PWMCH1 = 1;

float power1 = 0;            // サーボの出力Serial.printf("p%.2f,r%.2f,a%.2f,v%.2f\n", pitch, roll, arc, val);
float power2 = 0;
float power = 0;
float power0 = -4;
float power0_ = -5;              
float target = 0; // roll 54; //46// 83.0;  
float P, I, D, preP, dt;

//ジーグラー・ニコルズの手法で決定後Kd大きめに設定
float Ku = 54;float Td = 37;
float Kp = Ku*0.6;       //point2 // Pゲイン 全体的なモータのパワーに効く
float Ki = Ku*1.2/0.12;  // Iゲイン バランスが崩れて全体が平行移動しだしたときに効く
float Kd = 3*Ku*0.12/Td;  //point3 // Dゲイン 激しく動いたときのブレーキの役割をする

double roll, pitch;
float r_rad = 180 / PI;

float preTime;
#include <Ps3Controller.h>
int player = 0;

void notify()
{
    //--- Digital cross/square/triangle/circle button events ---
    if( Ps3.event.button_down.cross ) {
      Ku *=1.1;
      Serial.println(Ku);
      Serial.println("Change Ku up");
    }
    if( Ps3.event.button_down.square ){
      Td *=1.1;
      Serial.println(Td);
      Serial.println("Change Td up");
    }
    if( Ps3.event.button_down.triangle ){
      Ku *=0.9;
      Serial.println(Ku);
      Serial.println("Change Ku down");
    }
    if( Ps3.event.button_down.circle ){
      Td *=0.9;
      Serial.println(Td);
      Serial.println("Change Td down");
    }
    
    //--------------- Digital D-pad button events --------------
    if( Ps3.event.button_down.up ){
      power0 +=1;
      Serial.println(power0);
      Serial.println("Change power0 up");
    }

    if( Ps3.event.button_down.right ){
      power0_ +=1;
      Serial.println(power0_);
      Serial.println("Change power0_ up");
    }

    if( Ps3.event.button_down.down ){
      power0 -=1;
      Serial.println(power0);
      Serial.println("Change power0 down");
    }

    if( Ps3.event.button_down.left ){
      power0_ -=1;
      Serial.println(power0_);
      Serial.println("Change power0_ down");
    }
    Kp = Ku*0.6;       //point2 // Pゲイン 全体的なモータのパワーに効く
    Ki = Ku*1.2/0.12;  // Iゲイン バランスが崩れて全体が平行移動しだしたときに効く
    Kd = 3*Ku*0.12/Td;  //point3 // Dゲイン 激しく動いたときのブレーキの役割をする
    //Serial.printf("Ku%.2f,Td%.2f\n", Ku, Td); Serial.print("\n");
    
}

void onConnect(){
    Serial.println("Connected.");
}

void setup() {
  M5.begin(true, true, true);
  Serial.begin(115200);
  Ps3.attach(notify);
  Ps3.attachOnConnect(onConnect);
  Ps3.begin("94:b9:7e:92:4a:da");

  pinMode(PIN0, OUTPUT);
  pinMode(PIN1, OUTPUT);
  ledcSetup(PWMCH, 50, 12); //12bit
  ledcAttachPin(PIN0, PWMCH);
  ledcSetup(PWMCH1, 50, 12); 
  ledcAttachPin(PIN1, PWMCH1);
 
  M5.IMU.Init();
  preTime = micros();                   // 時間記録
}

void loop() {
  M5.IMU.getAttitude(&pitch, &roll);
  //double arc = atan2(pitch, roll) * r_rad + 180;
  //double val = sqrt(pitch * pitch + roll * roll); //pitch * pitch + roll * roll
  //Serial.printf("p%.2f,r%.2f,a%.2f,v%.2f\n", pitch, roll, arc, val);Serial.print("\n");

  dt = (micros()-preTime)/1000000. ;
  preTime = micros();

  P  = (target - roll) / 90;      // target - roll -90~90を取るので180で割って-1.0~1.0にする
  I += P * dt;                    // 偏差を積分する
  D  = (P - preP) / dt;           // 偏差を微分する
  preP = P;                       // 偏差を記録する
  if (abs(Ki*I)>90) I=0;

  power = Kp * P + Ki * I + Kd * D;
  power = constrain(power, -90, 90);  // ±90に制限
  //Serial.print("power =\t");  Serial.print(pSerial.printf("p%.2f,r%.2f,a%.2f,v%.2f\n", pitch, roll, arc, val);ower);  Serial.print("\n");

  power1 = 0 - power; // -power0+power0_; //point4
  power2 = 0 + power; // +power0+power0_; //回転方向注意
  servo_write(PWMCH, power1); 
  servo_write(PWMCH1, power2);

  //Serial.printf("%5.1f\n",roll); Serial.print("\n");
  //delay(10);
}

void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
  ang = map(ang, -90, 90, int(0.7*4096/20), int(2.3*4096/20)); //角度（-90～90）をPWMのパルス幅（700～2300）に変換
  ledcWrite(ch, ang);
}
```
###結果
倒立はほぼ出来たかなというレベルです。
無条件で、ずっと倒立を安定するところまでは出来ていません。
原因は、
・バラックな作りでm5atom不安定
・m5atomの角度揺らぎを抑えるコードを入れていない
・重心を考慮したパラメータを追求していない
などが考えられます。

とはいえ、一応らしい動画が取れたので以下に置いておきます。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr"><a href="https://twitter.com/hashtag/M5ATOM?src=hash&amp;ref_src=twsrc%5Etfw">#M5ATOM</a> <br>倒立振り子<br>なんとなくひも付きで、ここまで来た<br>しかし、もう少しバランスも考えないと、...だがもうこの辺りで止めようと思っているところ <a href="https://t.co/v8gvLkDfed">pic.twitter.com/v8gvLkDfed</a></p>&mdash; ウワン (@MuAuan) <a href="https://twitter.com/MuAuan/status/1401873551729197057?ref_src=twsrc%5Etfw">June 7, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
###まとめ
・m5atomを利用してみた
・倒立振り子をPS3コントローラーで制御するところまで出来た

・倒立振り子のさらなる安定化を目指す
・まだまだm5atomの機能を一部しか利用していないので、いろいろ追求したいと思う