とっくんラボ

電子工作、プログラミングで日々の発見や成果をよろづに

ESP32 (ESP-WROOM-32)でSDカードへ読み書きする。(Arduino IDE)

ESP32弄り第三弾は、SDカードへの読み書きです。

SDカードはSPIを使って通信します。

今回はこちらのSDカードモジュールを使用しました。

ピン配置は、


f:id:tocknblog:20170312021909p:plain


このようになっています。

ESP32への配線は以下の通り行いました。

  • DAT2 -> 無し
  • CD/DAT3 -> GPIO5
  • CMD -> GPIO23
  • VDD -> +3.3V
  • CLK -> GPIO18
  • VSS -> GND
  • DAT0 -> GPIO19
  • 以下接続なし

ちなみに、どうにかすれば接続するピンを変えられるみたいです。多分。


また、先に言っておきますが、絶対にいらないSDカードを使用してください。中身が飛ぶことがよくあります。
そして配線は慎重に行ってください。3.3V以上の電圧をかけるとSDカードがぶっ壊れると聞いたことがあります。
そして、SDカードの抜き差しは電源が入っていない状態で行ってください。SDカードを読み込まなくなったりします。
何か起きても私は責任トリマセン…


スケッチは、公式のExampleをそのまま使いました。ここにあります。

これを使えばSDカードの読み書きに関する大抵のことはできると思います。

もしライブラリを追加する方法がわからない~とかでしたら、コメントください。

上手く行けば実行結果はこんな感じになると思います。

SD Card Type: SDSC
SD Card Size: 1875MB
Listing directory: /
  DIR : /SYSTEM~1
  FILE: /YEAH.TXT  SIZE: 8
  FILE: /TEST.TXT  SIZE: 1048576
  FILE: /FOO.TXT  SIZE: 13
  DIR : /MYDIR
  DIR : /TESTDIR
Creating Dir: /mydir
Dir created
Listing directory: /
  DIR : /SYSTEM~1
  FILE: /YEAH.TXT  SIZE: 8
  FILE: /TEST.TXT  SIZE: 1048576
  FILE: /FOO.TXT  SIZE: 13
  DIR : /MYDIR
  DIR : /TESTDIR
Removing Dir: /mydir
rmdir failed
Listing directory: /
  DIR : /SYSTEM~1
Listing directory: /SYSTEM~1
  FILE: /SYSTEM~1/WPSETT~1.DAT  SIZE: 12
  FILE: /SYSTEM~1/INDEXE~1  SIZE: 76
  FILE: /YEAH.TXT  SIZE: 8
  FILE: /TEST.TXT  SIZE: 1048576
  FILE: /FOO.TXT  SIZE: 13
  DIR : /MYDIR
Listing directory: /MYDIR
  DIR : /MYDIR/TEEEEE
Listing directory: /MYDIR/TEEEEE
  DIR : /MYDIR/TEEEEE/SDF
  DIR : /TESTDIR
Listing directory: /TESTDIR
Writing file: /hello.txt
File written
Appending to file: /hello.txt
Message appended
Reading file: /hello.txt
Read from file: Hello World!
Deleting file: /foo.txt
File deleted
Renaming file /hello.txt to /foo.txt
File renamed
Reading file: /foo.txt
Read from file: Hello World!
1048576 bytes read for 3219 ms
1048576 bytes written for 5442 ms


SDカードのルートディレクトリにFOO.TXT、TEST.TXTというファイルが生成され、FOO.TXTの中にHello World!と書いてあれば成功だと思います。

以上!

ESP32 (ESP-WROOM-32)でMPU6050を使おう!(i2C) (Arduino IDE)

ESP32弄り日記第二弾は、安くて有名なInvenSense製6軸センサ、MPU6050を使おう!です。

MPU6050には3軸加速度センサー、3軸ジャイロセンサーが積んであります。3+3=6軸センサーというわけです。

ESP32とMPU6050を用いて、角度計を作ってみましょう。
今回もArduino IDEを使って開発していきます。

私が使用したのは以下のモジュールです。

なにやら10DOFとか書いてありますが、結局使用するのはMPU6050だけなので気にしなくていいです。
これから購入するのであれば、

これでOKです。

まず配線ですが、ESP32は以前書いたように、SCL、SDAピンを対応している中から自由に選べます。

今回はSCLピンをGPIO25、SDAピンをGPIO26にしました。

するとブレッドボードはこんな具合になります。

f:id:tocknblog:20170311164837j:plain


いやぁ、、、わちゃわちゃしてるなぁ、、、

スケッチはこんな感じです。
(一応断っておきますが、私は素人です…こんな書き方はヤメロぉ!とかありましたらご指摘ください…)

#include <Wire.h>

#define MPU6050_ADDR         0x68 // MPU-6050 device address
#define MPU6050_SMPLRT_DIV   0x19 // MPU-6050 register address
#define MPU6050_CONFIG       0x1a
#define MPU6050_GYRO_CONFIG  0x1b
#define MPU6050_ACCEL_CONFIG 0x1c
#define MPU6050_WHO_AM_I     0x75
#define MPU6050_PWR_MGMT_1   0x6b

double offsetX = 0, offsetY = 0, offsetZ = 0;
double gyro_angle_x = 0, gyro_angle_y = 0, gyro_angle_z = 0;
float angleX, angleY, angleZ;
float interval, preInterval;
float acc_x, acc_y, acc_z, acc_angle_x, acc_angle_y;
float gx, gy, gz, dpsX, dpsY, dpsZ;

void culcRotation();

//I2c書き込み
void writeMPU6050(byte reg, byte data) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission();
}

//i2C読み込み
byte readMPU6050(byte reg) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(reg);
  Wire.endTransmission(true);
  Wire.requestFrom(MPU6050_ADDR, 1/*length*/); 
  byte data =  Wire.read();
  return data;
}

void setup() {

  Wire.begin(26, 25);
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  
  Serial.begin(9600);
  delay(100);
  
  //正常に接続されているかの確認
  if (readMPU6050(MPU6050_WHO_AM_I) != 0x68) {
    Serial.println("\nWHO_AM_I error.");
    while (true) ;
  }

  //設定を書き込む
  writeMPU6050(MPU6050_SMPLRT_DIV, 0x00);   // sample rate: 8kHz/(7+1) = 1kHz
  writeMPU6050(MPU6050_CONFIG, 0x00);       // disable DLPF, gyro output rate = 8kHz
  writeMPU6050(MPU6050_GYRO_CONFIG, 0x08);  // gyro range: ±500dps
  writeMPU6050(MPU6050_ACCEL_CONFIG, 0x00); // accel range: ±2g
  writeMPU6050(MPU6050_PWR_MGMT_1, 0x01);   // disable sleep mode, PLL with X gyro

  //キャリブレーション
  Serial.print("Calculate Calibration");
  for(int i = 0; i < 3000; i++){
    
    int16_t raw_acc_x, raw_acc_y, raw_acc_z, raw_t, raw_gyro_x, raw_gyro_y, raw_gyro_z ;
    
    Wire.beginTransmission(MPU6050_ADDR);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU6050_ADDR, 14, true);
  
    raw_acc_x = Wire.read() << 8 | Wire.read();
    raw_acc_y = Wire.read() << 8 | Wire.read();
    raw_acc_z = Wire.read() << 8 | Wire.read();
    raw_t = Wire.read() << 8 | Wire.read();
    raw_gyro_x = Wire.read() << 8 | Wire.read();
    raw_gyro_y = Wire.read() << 8 | Wire.read();
    raw_gyro_z = Wire.read() << 8 | Wire.read();
    dpsX = ((float)raw_gyro_x) / 65.5;
    dpsY = ((float)raw_gyro_y) / 65.5;
    dpsZ = ((float)raw_gyro_z) / 65.5;
    offsetX += dpsX;
    offsetY += dpsY;
    offsetZ += dpsZ;
    if(i % 1000 == 0){
      Serial.print(".");
    }
  }
  Serial.println();

  offsetX /= 3000;
  offsetY /= 3000;
  offsetZ /= 3000;

  Serial.print("offsetX : ");
  Serial.println(offsetX);
  Serial.print("offsetY : ");
  Serial.println(offsetY);
  Serial.print("offsetZ : ");
  Serial.println(offsetZ);
  
}

void loop() {

  calcRotation();

  Serial.print("angleX : ");
  Serial.print(angleX);
  Serial.print("    angleY : ");
  Serial.print(angleY);
  Serial.print("    angleZ : ");
  Serial.println(angleZ);
 
}

//加速度、ジャイロから角度を計算
void calcRotation(){

  int16_t raw_acc_x, raw_acc_y, raw_acc_z, raw_t, raw_gyro_x, raw_gyro_y, raw_gyro_z ;
  
  //レジスタアドレス0x3Bから、計14バイト分のデータを出力するようMPU6050へ指示
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 14, true);
  
  //出力されたデータを読み込み、ビットシフト演算
  raw_acc_x = Wire.read() << 8 | Wire.read();
  raw_acc_y = Wire.read() << 8 | Wire.read();
  raw_acc_z = Wire.read() << 8 | Wire.read();
  raw_t = Wire.read() << 8 | Wire.read();
  raw_gyro_x = Wire.read() << 8 | Wire.read();
  raw_gyro_y = Wire.read() << 8 | Wire.read();
  raw_gyro_z = Wire.read() << 8 | Wire.read();
  
  //単位Gへ変換
  acc_x = ((float)raw_acc_x) / 16384.0;
  acc_y = ((float)raw_acc_y) / 16384.0;
  acc_z = ((float)raw_acc_z) / 16384.0;
  
  //加速度センサーから角度を算出
  acc_angle_y = atan2(acc_x, acc_z + abs(acc_y)) * 360 / -2.0 / PI;
  acc_angle_x = atan2(acc_y, acc_z + abs(acc_x)) * 360 / 2.0 / PI;

  dpsX = ((float)raw_gyro_x) / 65.5; // LSB sensitivity: 65.5 LSB/dps @ ±500dps
  dpsY = ((float)raw_gyro_y) / 65.5;
  dpsZ = ((float)raw_gyro_z) / 65.5;
  
  //前回計算した時から今までの経過時間を算出
  interval = millis() - preInterval;
  preInterval = millis();
  
  //数値積分
  gyro_angle_x += (dpsX - offsetX) * (interval * 0.001);
  gyro_angle_y += (dpsY - offsetY) * (interval * 0.001);
  gyro_angle_z += (dpsZ - offsetZ) * (interval * 0.001);
  
  //相補フィルター
  angleX = (0.996 * gyro_angle_x) + (0.004 * acc_angle_x);
  angleY = (0.996 * gyro_angle_y) + (0.004 * acc_angle_y);
  angleZ = gyro_angle_z;
  gyro_angle_x = angleX;
  gyro_angle_y = angleY;
  gyro_angle_z = angleZ;
}




まず最初のsetup()内のfor文ですが、これでジャイロセンサーのドリフト誤差を抑える為の計算をしています。

ドリフト誤差とはなんぞ?という方のために…
ジャイロセンサとは、つまりは角速度センサなのであります。角速度から角度を得るには…そう、積分ですよね!
プログラム内では、毎回毎回ジャイロセンサからの出力値と時間(秒)を掛け合わせたものを足していき、それが出力する角度としています。

理想的なジャイロセンサならば静止状態の時の出力値はゼロなのですが、世の中そうは行きません。普通は、静止状態でも常にゼロ以外の小さな値を出力し続けてしまいます。
放っておくと、どうなるでしょうか。
エス、どんどん角度が増えて(減って)いってしまう!!!
これをドリフト誤差といいます。

それを防ぐために、静止状態で出力し続ける値を1000回位読み取って平均値を取り、その値を積分時に生の値から引いているのです。


さて次は、一番下の関数calcRotation()に注目してください。

その一部分

  raw_acc_x = Wire.read() << 8 | Wire.read();
  raw_acc_y = Wire.read() << 8 | Wire.read();
  raw_acc_z = Wire.read() << 8 | Wire.read();
  raw_t = Wire.read() << 8 | Wire.read();
  raw_gyro_x = Wire.read() << 8 | Wire.read();
  raw_gyro_y = Wire.read() << 8 | Wire.read();
  raw_gyro_z = Wire.read() << 8 | Wire.read();

ここ、何だこれって思いませんか?私は思います。

MPU6050は、各センサの出力値を

x軸加速度、y軸加速度、z軸加速度、温度、x軸角速度、y軸角速度、z軸角速度

の順番に出力しますが、さらにその各データは上位8bit→下位8bitの順に出力されています。つまり計14個のデータですね。
そこで、上記の部分ではビットシフト演算を行い、正しい値へ変換しているのです。


そしてその下、「単位Gへ変換」という部分ですが、MPU6050は出力した加速度センサのデータを、16384で割ることで単位Gへ変換することが出来ます。
ただしこれは設定で変更することが出来ます。
ちゃっかりその設定を、最初のsetup()内で行っていました。

  //設定を書き込む
  writeMPU6050(MPU6050_SMPLRT_DIV, 0x00);   // sample rate: 8kHz/(7+1) = 1kHz
  writeMPU6050(MPU6050_CONFIG, 0x00);       // disable DLPF, gyro output rate = 8kHz
  writeMPU6050(MPU6050_GYRO_CONFIG, 0x08);  // gyro range: ±500dps
  writeMPU6050(MPU6050_ACCEL_CONFIG, 0x00); // accel range: ±2g
  writeMPU6050(MPU6050_PWR_MGMT_1, 0x01);   // disable sleep mode, PLL with X gyro

ここです。(上から3,4行目)
今回は加速度センサは±2G、ジャイロセンサは±500dps(degree per second)まで計測できる設定にしています。
ここの設定によって、取得した生のデータから変換するための計算式が変わってくるのです。
詳しくはデータシートを見てネ!

その下

  //加速度センサーから角度を算出
  acc_angle_y = atan2(acc_x, acc_z + abs(acc_y)) * 360 / -2.0 / PI;
  acc_angle_x = atan2(acc_y, acc_z + abs(acc_x)) * 360 / 2.0 / PI;

こちらは書いてある通り、逆三角関数を用いて角度を計算しています。
選ぶ軸と正負には気をつけてね!!

  dpsX = ((float)raw_gyro_x) / 65.5; // LSB sensitivity: 65.5 LSB/dps @ ±500dps
  dpsY = ((float)raw_gyro_y) / 65.5;
  dpsZ = ((float)raw_gyro_z) / 65.5;

ジャイロセンサーから取得した生のデータを、dps (degree per second)へ変換しています。加速度のときと同じですね。

  //前回計算した時から今までの経過時間を算出
  interval = millis() - preInterval;
  preInterval = millis();
  
  gyro_angle_x += (dpsX - offsetX) * (interval * 0.001);
  gyro_angle_y += (dpsY - offsetY) * (interval * 0.001);
  gyro_angle_z += (dpsZ - offsetZ) * (interval * 0.001);

ここでは数値積分を行っています。最初に計算したoffset達がここで役立つわけなんです!

millis()は、ESP32の起動から今までの経過時間を単位ミリ秒で取得できます。interval * 0.001は、ミリ秒から秒へ変換しているわけです。

そして最後の

  //相補フィルター
  angleX = (0.996 * gyro_angle_x) + (0.004 * acc_angle_x);
  angleY = (0.996 * gyro_angle_y) + (0.004 * acc_angle_y);
  angleZ = gyro_angle_z;
  gyro_angle_x = angleX;
  gyro_angle_y = angleY;
  gyro_angle_z = angleZ;

ここでは相補フィルターというものを使い、加速度センサーから求めた角度と、ジャイロセンサーから求めた角度を組み合わせています。
なぜそんなことをする必要があるのか、それは、互いの欠点を打ち消し合い、長所を活かしているのです。

まずジャイロセンサーには前述の通り、ドリフト誤差という欠点があります。一応このプログラムではそれを出来る限り抑えるようにしていますが、それでもやはりドリフトしてしまいます。

そして加速度センサーは、角度を加速度で求めているというのが欠点になります。なぜなら、静止状態なら良いのですが、対象が動く物体だったらどうでしょう。激しく動けば動くほど、あらゆる軸に加速度がかかり、角度がブレてしまうのです。

まとめると、

  • 加速度センサーは長時間運用に強いが、瞬間的なもの(外力)には弱い

そこで

 angleX = (0.996 * gyro_angle_x) + (0.004 * acc_angle_x);
  angleY = (0.996 * gyro_angle_y) + (0.004 * acc_angle_y);

この式なのです。
瞬間的にはジャイロセンサーの値を適用するが、ほぼ静止するとじわじわと加速度センサーの値へ近づいていくのがわかると思います。
これが相補フィルターなのです。(多分)
他にもNASAの?何かの?姿勢制御に使われた?カルマンフィルターなどがあるそうなのですが?、よくわかりませんでした。
こういったセンサーを組み合わせることを、センサフュージョンというらしいです。かっこいい!


説明は大体こんな感じでしょうか。


f:id:tocknblog:20170311175829p:plain


ちゃんと書き込めれば、こんな感じになると思います。

何か質問、ご指摘等ありましたら、気軽にコメントしてください!

以上!

ESP32 (ESP-WROOM-32)でi2C通信


2017/03/12追記:実際に使ってみました


先日、Arduinoの環境で開発できるということで、ESP32を購入しました。

 

私はATmega328(Arduino UNO)ではi2Cを頻繁に使用していたので、早速購入したESP32でもi2Cを使ってみました。

しかし、躓いた点があったので報告します。

 

Arduinoでi2C通信する際に欠かせないWireライブラリですが、最初に

Wire.begin();

を実行しますよね。

ATmegaではi2Cで使うSCL, SDAピンは固定されていますが、ESP32では対応している複数のピンの中から2つを選んで使うことができます。そのため、まず最初に使用するピンを指定しなければなりませんでした。

ESP32でWireライブラリを使う場合、SCL, SDAピンの指定は

int SDA = 25;
int SCL = 26;

Wire.begin(SDA, SCL);


のように、beginの引数にSDA、SCLの順にピンの番号を入れることで出来ます。

i2Cで使えるセンサーを使う時、データシートをみながらプログラムをコツコツ書かずに、誰かが作ったライブラリを用いることがあると思います。その場合にも、ライブラリ内のbegin()の部分を書き換える必要がありそうです。

以上!