【M5StickC入門】内蔵MPU6886と１６chServoコントローラPCA9685を利用して追跡カメラを制御する♪

今回は、巷でコンパクトで使いやすいと有名なM5StickCを利用して、追跡カメラを制御してみた。
M5StickCの仕様は以下のとおりです
最初の絵のようにいろいろつかえそうです。
ベースは、ESP32　PICO-４Mということで、今まで扱ってきたマイコンの線上にあるものだと分かります．
・ArduinoIDEやMicroPythonで開発出来る
・８０mAhバッテリーも内蔵
・Wifi対応
・赤外線
・MIC
・６軸IMUセンサー
・LCD
などなど
![m5stickc_2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/b0fc2ecf-ac54-bf3d-40da-88660f4194cf.png)
以下が、裏面拡大図です.
![m5stickc_04_7xQIo1hUjf.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/2870a297-7f39-9bc1-7c3e-c4232c50420d.png)
今回は、内蔵IMUセンサーと2個のServoを独立に制御するために、PCA9685をI2Cで利用するために、SLC（G26Pin）, SDA（G0Pin）及びGROVEのGNDとVoutを利用しました。
![PCA9685-Diagram.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/5ad3dfc3-ff66-b4ea-429f-adc9fd4b9667.jpeg)

###・接続
・（SDA, SCL）＝（G0,G26）または(G32, G33)
・Wire.begin(0,26)　またはWire.begin(32,33); の宣言が必要

|M5StickC|mpu6886(内蔵)|PCA9685|Servo|comment|
|:-:|:-:|:-:|:-:|:--:|
|GND||GND|||
|3.3V||VCC||PCA9685led点灯|
|SCL（G26）|SCL(内部)|SCL||GROVEG33も可|
|SDA（G0）|SDA（内部）|SDA||GROVEG32も可|
|||0_(yellow)|Servo1(yellow)||
|||0_(Black)|Servo1(Black)||
|||0_(red)|Servo1(red)||
|||...|...|16台制御可|
|||15_(yellow)|Servo2(yellow)||	
|||15_(Black)|Servo2(Black)||
|||15_(red)|Servo2(red)||
|PILO(4.2v)+||+||ServoPower+|
|PILO(4.2v)-||-||ServoPower-|


###・Servoを動かす
####・IO26利用
IO26のPWM信号を分けて二つのサーボに入力すれば二個動く。
[M5StickCとサーボでつくる、API連動のかっこいいアナログメーター](https://slanew.com/news/2387)
結線

|M5StickC|Servo|comment|
|:-:|:-:|:--:|
|GND|GND|GND共通|
|IO26|yellow||
|PILO(4.2v)+|+|外部電源|
|PILO(4.2v)-|-|外部電源|

<details><summary>m5stickcでservo制御のためのコード</summary><div>

```servo_m5.ino.c
#include <Servo.h>
#include <M5StickC.h>　

static const int servoPin1 = 26;
Servo servo1;

/*
Geekservo（灰色）のパルス間隔範囲は500〜2500。
最大パルスの位置を0にあわせ、最小パルスの数値を調整してメーターの最大値に合わせる。
https://akizukidenshi.com/download/ds/towerpro/SG90_a.pdf
*/
const int channel = -1;
const int minAngle = -90;          // 最小アングル（ステップ）
const int maxAngle = 90;        // 最大アングル（ステップ）
const int minPalseWidth = 500;   // 最小パルス間隔（メーターの最大位置：この値を調節する）
const int maxPalseWidth = 2400;  // 最小パルス間隔（メーターの最小位置）

void setup() {
  servo1.attach(servoPin1,channel,minAngle,maxAngle,minPalseWidth,maxPalseWidth); 

  M5.begin();
  M5.Lcd.setRotation(3);
  M5.Lcd.setTextSize(4);
  M5.Lcd.setCursor(0, 0, 2);
  M5.Lcd.println("Ready");
}

void loop() {
  for(int posDegrees = -90; posDegrees <= 90; posDegrees++) {
      servo1.write(posDegrees);
      M5.Lcd.fillScreen(BLACK);
      M5.Lcd.setCursor(0, 0, 2);
      M5.Lcd.println(posDegrees);
      delay(100);
  }
  delay(1000);
  for(int posDegrees = 90; posDegrees >= -90; posDegrees--) {
      servo1.write(posDegrees);
      M5.Lcd.fillScreen(BLACK);
      M5.Lcd.setCursor(0, 0, 2);
      M5.Lcd.println(posDegrees);
      delay(100);
  }
}
```
</div></details>
####PWM(LED Control)で複数サーボを動かす

[PWM(LED Control)@M5StickC非公式日本語リファレンス](https://lang-ship.com/reference/unofficial/M5StickC/Peripherals/LED_Control/)
[ESP32のPWM出力は255が最大じゃなかった](https://lang-ship.com/blog/work/esp32-pwm-max/)
[Arduino – ESP32 の PWM ( LEDC )で 40MHzまでの安定した高周波パルスを思い通りに出せたぞ](https://www.mgo-tec.com/blog-entry-ledc-pwm-arduino-esp32.html)

結線

|M5StickC|PILO(4.2v)|Servo|comment|
|:-:|:-:|:--:|:--:|
|GND|GND|GND|GND共通|
|IO26||servo0;yellow|PWM信号線|
||+|servo0;red+|外部電源＋|
|GND|-|servo0;black-|外部電源−|

※注意；m5stickcの電源出力でサーボ二個接続だと内部回路が壊れました

上記参考より引用（ただし、Servo駆動に応用出来るという記載はありません）
「3.3V出力をON/OFFすることで擬似的に出力を制御するPWMを利用することができます。」

|PIN|IO26|IO36|IO0|IO32|IO33|
|:-:|:-:|:--:|:-:|:-:|:--:|
|ledcWrite()|○|NG|○|○|○|

<details><summary>PWM(LED Control)でservo制御のためのコード</summary><div>

```pwmled.ino.c
#include <M5StickC.h>

int PIN = 26;
int PWMCH = 0;

void setup() {
  M5.begin();

  pinMode(PIN, OUTPUT);
  ledcSetup(PWMCH, 50, 12); //12bit 4096
  ledcAttachPin(PIN, PWMCH);
}

void loop() {
  ledcWrite(PWMCH, 103);    //   0.5msec(-90) for sg90
  delay(500);
  ledcWrite(PWMCH, 246);  //  1.2msec(0) for sg90
  delay(500);
  ledcWrite(PWMCH, 491);  // 2.4msec(90) for sg90
  delay(500);
}
```
</div></details>

|M5StickC|PILO(4.2v)|Servo|comment|
|:-:|:-:|:--:|:--:|
|GND|GND|GND|GND共通|
|IO26||servo0;yellow|PWM信号線|
|IO0||servo1;yellow|PWM信号線|
||+|servo0;red+|外部電源＋|
||+|servo1;red+|外部電源＋|
|GND|-|servo0;black-|外部電源−|
|GND|-|servo1;black-|外部電源−|

<details><summary>PWM(LED Control)で複数servo制御のためのコード</summary><div>

```c
#include <M5StickC.h>

int PIN0 = 26;
int PIN1 = 0;
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() {
  ledcWrite(PWMCH, 103);    //   0.5msec(-90)
  ledcWrite(PWMCH1, 103);    //   0.5msec(-90)
  delay(1000);
  ledcWrite(PWMCH, 123);  // 246 = 1.2msec(0)
  ledcWrite(PWMCH1, 123);    //   0.5msec(-90)
  delay(1000);
  ledcWrite(PWMCH, 246);  // 491 = 2.4msec(90)
  ledcWrite(PWMCH1, 246);    //   0.5msec(-90)
  delay(1000);
}
```
</div></details>
####PCA9685を利用して複数Servo制御
[M5StickCでI2C通信をする](https://lang-ship.com/blog/work/m5stickc-i2c/)
[四足歩行ロボットを作りたい！～Arduino・I2C通信　で複数サーボを動かすdo～](https://fun-desier-blog.com/%E5%9B%9B%E8%B6%B3%E6%AD%A9%E8%A1%8C%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88%E3%82%92%E4%BD%9C%E3%82%8A%E3%81%9F%E3%81%84%EF%BC%81%EF%BD%9Earduino%E3%83%BBi2c%E9%80%9A%E4%BF%A1%E3%80%80%E3%81%A7%E8%A4%87/)

結線

|M5StickC|PILO(4.2v)|PCA9685|Servo|comment|
|:-:|:-:|:-:|:-:|:--:|
|GROVE GND||GND|||
|GROVE 3.3V||VCC||PCA9685led点灯|
|SCL（G26orG33）||SCL||GROVE G33も可|
|SDA（G0orG32）||SDA||GROVE G32も可|
|||0_(yellow)|Servo1(yellow)||
|||0_(Black)|Servo1(Black)||
|||0_(red)|Servo1(red)||
|||...|...|合計16台制御可|
|||15_(yellow)|Servo2(yellow)||	
|||15_(Black)|Servo2(Black)||
|||15_(red)|Servo2(red)||
||＋|+||PCA外部電源+入力|
||−|-||PCA外部電源-入力|


PCA9685のパッケージはいろいろあるが、最終的にAdafruit_PWMServoDriverが動いた.
[Adafruit-PWM-Servo-Driver-Library](https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library)
install;ライブラリーマネジャーで検索してインストールが簡単

i2c通信の(SDA, SCL)の組み合わせについて、Wire.begin(0,26); がとても重要で、(32.33)を指定すれば、こちらも使えるようになった。
PCA9685は、外部電源をつなげるタイプであり、今回はPILO(4.2V)を利用した。servoのコネクトもまんま3本させるので安定して使える。

<details><summary>PCA9685複数servo制御のためのコード</summary><div>

```pca9685_ex.ino.c
#include <M5StickC.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() {
 Wire.begin(0,26); //i2cのpin宣言が重要
 pwm.begin();
 pwm.setPWMFreq(50); // SG90は 50 Hz で動く（PWM周波数設定）
 
 M5.begin();
 M5.Lcd.setRotation(3);
 M5.Lcd.fillScreen(BLACK);
 M5.Lcd.setTextSize(1);
 M5.Lcd.setCursor(40, 0, 2);
 M5.Lcd.println("pca9685 TEST");
   
}

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

  sk += 1;
  
  M5.Lcd.setCursor(0, 15);
  M5.Lcd.printf("output start"); 
  M5.Lcd.printf("\n"); 
  M5.Lcd.printf("1*sk");
  M5.Lcd.printf("\n");

}

void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
  ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅（150～500）に変換
  pwm.setPWM(ch, 0, ang);
}
```
</div></details>
###内蔵MPU6886で加速度、角速度を測る
M5.Lcdへの表示以外は、`M5.MPU6886.getGyroData(&gyroX,&gyroY,&gyroZ);`や`M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);`などで取得出来るので簡単です。
madgwickも、[他のespマイコンで利用したもの](https://qiita.com/MuAuan/items/081488af923fcf4f87de)が使えました.

<details><summary>MPU6886で加速度・角速度計測のコード</summary><div>

```simple_mpu6886.ino.c
#include <M5StickC.h>
#include <MadgwickAHRS.h>   // 角度と角速度にMadgwickフィルタをかけるライブラリ
Madgwick MadgwickFilter;    // Madgwickフィルタのオブジェクトを設定

float roll, pitch, yaw;
float preTime;
float accX = 0, accY = 0, accZ = 0;
float accX0 = 0, accY0 = 0, accZ0 = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;
float yaw_ =0;

void setup() {
  M5.begin();
  M5.Lcd.setRotation(3);
  M5.Lcd.fillScreen(BLACK);
  
  M5.Lcd.setTextSize(1);
  M5.Lcd.setCursor(40, 0, 2);
  M5.Lcd.println("MPU6886 TEST");
  M5.MPU6886.Init();
  MadgwickFilter.begin(100);  // Madgwickフィルタの周波数を100Hzに設定。
  preTime = micros();                   // 時間を記録
}

void loop() {
  M5.MPU6886.getGyroData(&gyroX,&gyroY,&gyroZ);
  M5.MPU6886.getAccelData(&accX,&accY,&accZ);
  // Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
  MadgwickFilter.updateIMU(gyroX, gyroY, gyroZ, accX, accY, accZ);
  
  float pitch_  = MadgwickFilter.getPitch();
  float roll_  = MadgwickFilter.getRoll();
  //float dt = (micros()-preTime)/1000000. ;
  Serial.print((micros()-preTime)/1000);
  Serial.print("\n");
  preTime = micros();
  
  //yaw_  +=  gyroZ*dt;
  yaw_  =  MadgwickFilter.getYaw();

  M5.Lcd.setCursor(0, 15);
  M5.Lcd.printf("measurement start"); 
  M5.Lcd.printf("\n"); 
  M5.Lcd.printf("pitch,roll,yaw");
  M5.Lcd.printf("\n"); 
  M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);
  Serial.printf("%5.1f,%5.1f,%5.1f\n",pitch,roll,yaw);
  M5.Lcd.printf("%5.1f,%5.1f,%5.1f\n",pitch,roll,yaw);
  Serial.printf("%5.1f,%5.1f,%5.1f\n",pitch_,roll_,yaw_);
  M5.Lcd.printf("%5.1f,%5.1f,%5.1f\n",pitch_,roll_,yaw_);
  delay(0);
  Serial.print("\n");
}
```
</div></details>
###追跡カメラを制御する
これで、追跡カメラも動きました。
そして、マイコンを変えて同じテーマでいくつか実施して来たが、一番なめらかに動いてくれた。

[M5StickCの6軸IMU MPU6886を調べる](https://lang-ship.com/blog/work/m5stickc-imu-mpu6886/)
[M5StickCのIMU(AHRS)研究 その2 パラメータ調整](https://lang-ship.com/blog/work/m5stickc-imu-ahrs-2/)

接続は、冒頭に記載したとおり。

<details><summary>追跡カメラのコード</summary><div>

```pca9685_mpu6886.ino.c
#include <M5StickC.h>
#include <Wire.h> 
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#include <M5StickC.h>
#include <MadgwickAHRS.h>   // 角度と角速度にMadgwickフィルタをかけるライブラリ
Madgwick MadgwickFilter;    // Madgwickフィルタのオブジェクトを設定

#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;

float roll, pitch, yaw;
float preTime;
float accX = 0, accY = 0, accZ = 0;
float accX0 = 0, accY0 = 0, accZ0 = 0;

float gyroX = 0, gyroY = 0, gyroZ = 0;
float gyroX0 = 0, gyroY0 = 0, gyroZ0 = 0;
float gyro_x0 = 0, gyro_y0 = 0, gyro_z0 = 0;
float yaw_ =0;

void setup() {
  Wire.begin(32,33); 
  pwm.begin();
  pwm.setPWMFreq(50); // SG90は 50 Hz で動く（PWM周波数設定）
   
  M5.begin();
  M5.Lcd.setRotation(3);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextSize(1);
  M5.Lcd.setCursor(40, 0, 2);
  M5.Lcd.println("pca9685-MPU6886 TEST");
     
  M5.begin();
  M5.Lcd.setRotation(3);
  M5.Lcd.fillScreen(BLACK);
  
  M5.Lcd.setTextSize(1);
  M5.Lcd.setCursor(40, 0, 2);
  M5.MPU6886.Init();
  MadgwickFilter.begin(100);  // Madgwickフィルタの周波数を100Hzに設定。
  int count = 1000; //ドリフト抑えるために初期値測定
  for (int i = 0; i < count; i++) {
    M5.MPU6886.getGyroData(&gyroX,&gyroY,&gyroZ);
    gyro_x0 += gyroX;
    gyro_y0 += gyroY;
    gyro_z0 += gyroZ;
  }
  //係数はyaw_のドリフト最小で設定
  gyroX0, gyroY0, gyroZ0 = -.87*gyro_x0/count, -.87*gyro_y0/count, .8*gyro_z0/count;
  M5.MPU6886.getAccelData(&accX0,&accY0,&accZ0);
  preTime = micros();                   // 時間を記録
}

void loop() {
  M5.MPU6886.getGyroData(&gyroX,&gyroY,&gyroZ);
  M5.MPU6886.getAccelData(&accX,&accY,&accZ);
  // Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
  MadgwickFilter.updateIMU(gyroX-gyroX0, gyroY-gyroY0, gyroZ-gyroZ0, accX-accX0, accY-accY0, accZ-accZ0+1);
  //MadgwickFilter.updateIMU(gyroX, gyroY, gyroZ, accX, accY, accZ);

  float pitch_  = MadgwickFilter.getPitch();
  float roll_  = MadgwickFilter.getRoll();
  float dt = (micros()-preTime)/1000000. ;
  //Serial.print((micros()-preTime)/1000);
  //Serial.print("\n");
  preTime = micros();
  
  //yaw_  +=  gyroZ*dt;  //MadgwickFilter.getYaw();
  yaw_  =  MadgwickFilter.getYaw();
  
  M5.Lcd.setCursor(0, 15);
  M5.Lcd.printf("measurement start"); 
  M5.Lcd.printf("\n"); 
  M5.Lcd.printf("pitch,roll,yaw");
  M5.Lcd.printf("\n"); 
  M5.MPU6886.getAhrsData(&pitch, &roll, &yaw);
  //Serial.printf("%5.1f,%5.1f,%5.1f\n",pitch,roll,yaw-180);
  M5.Lcd.printf("%5.1f,%5.1f,%5.1f\n",pitch,roll,yaw-180);
  Serial.printf("%5.1f,%5.1f,%5.1f\n",pitch_,roll_,yaw_-180);
  M5.Lcd.printf("%5.1f,%5.1f,%5.1f\n",pitch_,roll_,yaw_-180);
  delay(0);
  Serial.print("\n");
  //servo_write(0, pitch+90); //pitchはドリフト小なのでyaw使えないとき
  servo_write(0, yaw_-180+90);
  servo_write(15, roll+90);
  sk += 1;
}

void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
  ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅（150～500）に変換
  pwm.setPWM(ch, 0, ang);
}
```
</div></details>
上記でほぼ追跡カメラ完成だが、まだまだ実運用にはyawのドリフトが大きい。
###まとめ
・m5stickcで追跡カメラを制御した
・複数servoを動かせるようになった
・内蔵MPU6886を使えるようになった
・i2c通信の使い方を理解した

・もう少しドリフトを抑える工夫をしよう
・異なるおもちゃを作ろう