LoginSignup
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@MuAuan(ムー ウワン)

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

Last updated at Posted at 2021-06-13

前回、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

pin配置は以下の背面図が分かり易いです。
m5atom.jpg
予約されているPinは以下のとおりです。
（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/の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/
※以下のコードの他に、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を利用しました。
しかし、コードは以下のとおり、少し異なります。
呼び出しが、m5stickcではM5.MPU6886.Init();のようになっていましたが、m5atomではM5.IMU.Init();となっています。
また、M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);の関数もM5.IMU.getAttitude(&pitch, &roll);を利用するようです。
※さらなる参考として以下をあげておきます.
【参考】
カルマン・フィルタで ATOM Matrix 傾斜計 ーリアクションホイールへの道3ー

M5Atom/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と同様に、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にインストールした書込みツールを利用して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の角度揺らぎを抑えるコードを入れていない
・重心を考慮したパラメータを追求していない
などが考えられます。

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

### まとめ ・m5atomを利用してみた ・倒立振り子をPS3コントローラーで制御するところまで出来た

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

1
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?