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

今回は、巷でコンパクトで使いやすいと有名なM5StickCを利用して、追跡カメラを制御してみた。
M5StickCの仕様は以下のとおりです
最初の絵のようにいろいろつかえそうです。
ベースは、ESP32　PICO-４Mということで、今まで扱ってきたマイコンの線上にあるものだと分かります．
・ArduinoIDEやMicroPythonで開発出来る
・８０mAhバッテリーも内蔵
・Wifi対応
・赤外線
・MIC
・６軸IMUセンサー
・LCD
などなど

以下が、裏面拡大図です.

今回は、内蔵IMUセンサーと2個のServoを独立に制御するために、PCA9685をI2Cで利用するために、SLC（G26Pin）, SDA（G0Pin）及びGROVEのGNDとVoutを利用しました。

・接続

・（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連動のかっこいいアナログメーター
結線

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

m5stickcでservo制御のためのコード

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);
  }
}

PWM(LED Control)で複数サーボを動かす

PWM(LED Control)@M5StickC非公式日本語リファレンス
ESP32のPWM出力は255が最大じゃなかった
Arduino – ESP32 の PWM ( LEDC )で 40MHzまでの安定した高周波パルスを思い通りに出せたぞ

結線

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	○	○	○

PWM(LED Control)でservo制御のためのコード

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);
}

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-	外部電源−

PWM(LED Control)で複数servo制御のためのコード

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

PCA9685を利用して複数Servo制御

M5StickCでI2C通信をする
四足歩行ロボットを作りたい！～Arduino・I2C通信　で複数サーボを動かすdo～

結線

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
install;ライブラリーマネジャーで検索してインストールが簡単

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

PCA9685複数servo制御のためのコード

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);
}

内蔵MPU6886で加速度、角速度を測る

M5.Lcdへの表示以外は、M5.MPU6886.getGyroData(&gyroX,&gyroY,&gyroZ);やM5.MPU6886.getAhrsData(&pitch, &roll, &yaw);などで取得出来るので簡単です。
madgwickも、他のespマイコンで利用したものが使えました.

MPU6886で加速度・角速度計測のコード

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");
}

追跡カメラを制御する

これで、追跡カメラも動きました。
そして、マイコンを変えて同じテーマでいくつか実施して来たが、一番なめらかに動いてくれた。

M5StickCの6軸IMU MPU6886を調べる
M5StickCのIMU(AHRS)研究 その2 パラメータ調整

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

追跡カメラのコード

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);
}

上記でほぼ追跡カメラ完成だが、まだまだ実運用にはyawのドリフトが大きい。

まとめ

・m5stickcで追跡カメラを制御した
・複数servoを動かせるようになった
・内蔵MPU6886を使えるようになった
・i2c通信の使い方を理解した

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