esp32 と９軸センサーとサーボ

esp32 と 9軸センサー と サーボモータ

esp32 は手軽にIOT用のデバイスが作成できます。9軸センサーとサーボモータによるテスト用のプログラムを記述したので、ハードウェアとソフトウェアについて解説を記述します。

ハードウェア構成

以下の、パーツを購入することで手軽に動作させることができると思います。

1. ESP-WROOM-32D開発ボード
2. BMX055使用９軸センサーモジュール
3. マイクロサーボ SG-90
4. ブレッドボード 6穴版 EIC-3901
5. ブレッドボード・ジャンパーワイヤー
6. USBケーブル(ミニ - Aオス)

ESP-WROOM-32D開発ボード

WifiやBLEもできる、最近流行っているボードです。結構いろいろできるみたいなので、持っているのも悪くないと思います。

9軸センサー

加速度・ジャイロ・磁気の９軸センサーです。esp32 のI/O は3.3v みたいなので、JP7をショートさせて使用します。

サーボモータ

ホビー用によく使用される、サーボモータです。ソフトウェアでは、４個使用していますが、予算の都合で個数を購入するのがよいと思います。

ブレッドボード

ESP32 用には 一列が ６ピンないと、結線できないのでこちらを使用します。

ブレッドボード用ジャンパーワイヤー

結線用のワイヤーです。

USBケーブル

ESP32とPCを接続するケーブルです。持っていれば別に必要ではないです。

配線

以下、配線になります。モータ用電源に別途、電池ボックスとブレッドボードを使用しています。

電源ライン
3.3 V と GNDを ブレッドボードの＋－に接続します。
サーボモータ
GPIO32, 33, 25, 26 をサーボモータの信号用に使用します。
ここでは、電池ボックスから4.8v の電圧を生成して結線しています。
サーボモータが１・２個なら esp32 の 5Vを使用します。
９軸センサー
電源は上記の電源ラインから配線します。
信号用にはGPIO22, 21 は 加速度センサーとの接続用に使用します。

ソフトウェア

Arduino IDE を使用して開発しています。導入方法等は他のページを参照してください。

ソースコード

以下、全ソースコードです。加速度センサーから読み取った値から roll, pitch を計算しています。roll, pitch に連動してサーボモータを動作させています。
細かい解説は後ほど、追記します。

#include <Wire.h>
#include <MadgwickAHRS.h>
#include <ESP32Servo.h>
#include "esp_system.h"
#define LED_PIN 32
#define SERVO_COUNT 4
int SERVO_PINS[SERVO_COUNT] = { 33, 25, 26, 27 };
Servo myServo[SERVO_COUNT];
Madgwick MadgwickFilter;
#define BMX055_ACCL_ADDR 0x19
#define BMX055_GYRO_ADDR 0x69
#define BMX055_MAG_ADDR  0x13

void setup()
{
  Serial.begin(115200);
  while (!Serial);
  Serial.println("BMX055 Test setup start");
  myServo[0].attach(SERVO_PINS[0]);
  myServo[1].attach(SERVO_PINS[1]);
  myServo[2].attach(SERVO_PINS[2]);
  myServo[3].attach(SERVO_PINS[3]);
  // ic2
  Wire.begin();
  InitAcclBMX055();
  InitGyroBMX055();
  InitMagBMX055();
  MadgwickFilter.begin(50); //50Hz
  Serial.println("BMX055 Test setup end");
}

void loop()
{
  //Serial.println("Read...");
  float ax, ay, az;
  ReadAcclBMX055(ax, ay, az);
  float gx, gy, gz;
  RaddGyroBMX055(gx, gy, gz);
  float mx, my, mz;
  ReadMagBMX055(mx, my, mz);
  // calculate roll/pitch/yaw
  //MadgwickFilter.updateIMU(gx, gy, gz, ax, ay, az);
  MadgwickFilter.update(gx, gy, gz, ax, ay, az, mx, my, mz);

  float pitch = MadgwickFilter.getPitch();
  float roll    = MadgwickFilter.getRoll();
  float yaw   = MadgwickFilter.getYaw();
  //Serial.print(pitch); Serial.print(",");
  //Serial.print(roll); Serial.println("");
  if( pitch > -90 && pitch < 90) {
    float angle = pitch + 90;
    myServo[0].write(angle);
    myServo[1].write(angle);
  } else {
    myServo[0].write(90);
    myServo[1].write(90);
  }
  if( roll > -90 && roll < 90) {
    float angle = roll + 90;
    myServo[2].write(angle);
    myServo[3].write(angle);
  } else {
    myServo[2].write(90);
    myServo[3].write(90);
  }

  delay(20);         
}
void  InitAcclBMX055()
{
  // 加速度
  Wire.beginTransmission(BMX055_ACCL_ADDR);
  Wire.write(0x0F); // Select PMU_Range register
  Wire.write(0x03);   // Range = +/- 2g
  Wire.endTransmission();
  delay(100);
 //------------------------------------------------------------//
  Wire.beginTransmission(BMX055_ACCL_ADDR);
  Wire.write(0x10);  // Select PMU_BW register
  Wire.write(0x08);  // Bandwidth = 7.81 Hz
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(BMX055_ACCL_ADDR);
  Wire.write(0x11);  // Select PMU_LPW register
  Wire.write(0x00);  // Normal mode, Sleep duration = 0.5ms
  Wire.endTransmission();
  delay(10);
}
void  InitGyroBMX055()
{
 //------------------------------------------------------------//
  Wire.beginTransmission(BMX055_GYRO_ADDR);
  Wire.write(0x0F);  // Select Range register
  Wire.write(0x04);  // Full scale = +/- 125 degree/s
  Wire.endTransmission();
  delay(100);
 //------------------------------------------------------------//
  Wire.beginTransmission(BMX055_GYRO_ADDR);
  Wire.write(0x10);  // Select Bandwidth register
  Wire.write(0x07);  // ODR = 100 Hz
  Wire.endTransmission();
  delay(100);
 //------------------------------------------------------------//
  Wire.beginTransmission(BMX055_GYRO_ADDR);
  Wire.write(0x11);  // Select LPM1 register
  Wire.write(0x00);  // Normal mode, Sleep duration = 2ms
  Wire.endTransmission();
  delay(100);
}
void  InitMagBMX055()
{
  Wire.beginTransmission(BMX055_MAG_ADDR);
  Wire.write(0x4B);  // Select Mag register
  Wire.write(0x83);  // Soft reset
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(BMX055_MAG_ADDR);
  Wire.write(0x4B);  // Select Mag register
  Wire.write(0x01);  // Soft reset
  Wire.endTransmission();
  delay(100);
  //------------------------------------------------------------//
  Wire.beginTransmission(BMX055_MAG_ADDR);
  Wire.write(0x4C);  // Select Mag register
  Wire.write(0x00);  // Normal Mode, ODR = 10 Hz
  Wire.endTransmission();
 //------------------------------------------------------------//
  Wire.beginTransmission(BMX055_MAG_ADDR);
  Wire.write(0x4E);  // Select Mag register
  Wire.write(0x84);  // X, Y, Z-Axis enabled
  Wire.endTransmission();
 //------------------------------------------------------------//
  Wire.beginTransmission(BMX055_MAG_ADDR);
  Wire.write(0x51);  // Select Mag register
  Wire.write(0x04);  // No. of Repetitions for X-Y Axis = 9
  Wire.endTransmission();
 //------------------------------------------------------------//
  Wire.beginTransmission(BMX055_MAG_ADDR);
  Wire.write(0x52);  // Select Mag register
  Wire.write(0x16);  // No. of Repetitions for Z-Axis = 15
  Wire.endTransmission();
}
void ReadAcclBMX055(float &xAccl, float& yAccl, float& zAccl)
{
  int data[6];
  for (int i = 0; i < 6; i++)
  {
    Wire.beginTransmission(BMX055_ACCL_ADDR);
    Wire.write((2 + i));// Select data register
    Wire.endTransmission();
    Wire.requestFrom(BMX055_ACCL_ADDR, 1);// Request 1 byte of data
    // Read 6 bytes of data
    // xAccl lsb, xAccl msb, yAccl lsb, yAccl msb, zAccl lsb, zAccl msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data to 12-bits
  xAccl = ((data[1] * 256) + (data[0] & 0xF0)) / 16;
  if (xAccl > 2047)  xAccl -= 4096;
  yAccl = ((data[3] * 256) + (data[2] & 0xF0)) / 16;
  if (yAccl > 2047)  yAccl -= 4096;
  zAccl = ((data[5] * 256) + (data[4] & 0xF0)) / 16;
  if (zAccl > 2047)  zAccl -= 4096;
  xAccl = xAccl * 0.0098; // renge +-2g
  yAccl = yAccl * 0.0098; // renge +-2g
  zAccl = zAccl * 0.0098; // renge +-2g
}
void RaddGyroBMX055(float& xGyro, float& yGyro, float& zGyro)
{
  int data[6];
  for (int i = 0; i < 6; i++)
  {
    Wire.beginTransmission(BMX055_GYRO_ADDR);
    Wire.write((2 + i));    // Select data register
    Wire.endTransmission();
    Wire.requestFrom(BMX055_GYRO_ADDR, 1);    // Request 1 byte of data
    // Read 6 bytes of data
    // xGyro lsb, xGyro msb, yGyro lsb, yGyro msb, zGyro lsb, zGyro msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  xGyro = (data[1] * 256) + data[0];
  if (xGyro > 32767)  xGyro -= 65536;
  yGyro = (data[3] * 256) + data[2];
  if (yGyro > 32767)  yGyro -= 65536;
  zGyro = (data[5] * 256) + data[4];
  if (zGyro > 32767)  zGyro -= 65536;
  xGyro = xGyro * 0.0038; //  Full scale = +/- 125 degree/s
  yGyro = yGyro * 0.0038; //  Full scale = +/- 125 degree/s
  zGyro = zGyro * 0.0038; //  Full scale = +/- 125 degree/s
}
void ReadMagBMX055(float& xMag, float& yMag, float& zMag)
{
  int data[8];
  for (int i = 0; i < 8; i++)
  {
    Wire.beginTransmission(BMX055_MAG_ADDR);
    Wire.write((0x42 + i));    // Select data register
    Wire.endTransmission();
    Wire.requestFrom(BMX055_MAG_ADDR, 1);    // Request 1 byte of data
    // Read 6 bytes of data
    // xMag lsb, xMag msb, yMag lsb, yMag msb, zMag lsb, zMag msb
    if (Wire.available() == 1)
      data[i] = Wire.read();
  }
  // Convert the data
  xMag = ((data[1] <<8) | (data[0]>>3));
  if (xMag > 4095)  xMag -= 8192;
  yMag = ((data[3] <<8) | (data[2]>>3));
  if (yMag > 4095)  yMag -= 8192;
  zMag = ((data[5] <<8) | (data[4]>>3));
  if (zMag > 16383)  zMag -= 32768;
}