LoginSignup
0
1

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(ムー ウワン)

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

Posted at

今回は、巷でコンパクトで使いやすいと有名なM5StickCを利用して、追跡カメラを制御してみた。
M5StickCの仕様は以下のとおりです
最初の絵のようにいろいろつかえそうです。
ベースは、ESP32　PICO-４Mということで、今まで扱ってきたマイコンの線上にあるものだと分かります．
・ArduinoIDEやMicroPythonで開発出来る
・８０mAhバッテリーも内蔵
・Wifi対応
・赤外線
・MIC
・６軸IMUセンサー
・LCD
などなど
m5stickc_2.png
以下が、裏面拡大図です.
m5stickc_04_7xQIo1hUjf.png
今回は、内蔵IMUセンサーと2個のServoを独立に制御するために、PCA9685をI2Cで利用するために、SLC（G26Pin）, SDA（G0Pin）及びGROVEのGNDとVoutを利用しました。
PCA9685-Diagram.jpg

・接続

・（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通信をする](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
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マイコンで利用したもの](https://qiita.com/MuAuan/items/081488af923fcf4f87de)が使えました.
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通信の使い方を理解した

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

0
1
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
0
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?