esp32 と 9軸センサー と サーボモータ
esp32 は手軽にIOT用のデバイスが作成できます。9軸センサーとサーボモータによるテスト用のプログラムを記述したので、ハードウェアとソフトウェアについて解説を記述します。
ハードウェア構成
以下の、パーツを購入することで手軽に動作させることができると思います。
- ESP-WROOM-32D開発ボード
- BMX055使用９軸センサーモジュール
- マイクロサーボ SG-90
- ブレッドボード 6穴版 EIC-3901
- ブレッドボード・ジャンパーワイヤー
- 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;
}
