ESP32で加速度センサの値に応じてモータ制御


はじめに

この記事はロボP Advent Calendar 2018の14日目の記事です。

加速度センサADXL345を使って、上下の動きに応じてモーター制御をしました。

きっかけ

どうしても空を飛びたかったので、ドローンを作りたいと思いました。

ひとまず、ホバリングできるこういうドローンを
http://amzn.asia/d/1ViG0kI
ESP32を使って制御することにしました。

つかったもの

ESP32 
ADXL345 (加速度センサ)
DRV8835 (モータドライバ)
3Vモータ

つまずいたところ

ADXL345の接続にはI2Cを使うのだが、ESP32で接続ポートが決まっていた。
GPIO22 → SCL GPIO21 → SDA に接続するとしっかり決まっている。

接続

ADXL345 ESP32
GND GND
VCC 3V
SCL GPIO22
SDA GPIO21
DRV8835 ESP32  モータ 
GND GND
VCC 3V
VM 3V
BIN1 GPIO25
AIN1 GPIO25
BIN2 GPIO26
AIN2 GPIO26
AOUT1 +
BOUT1 +
AOUT2 -
BOUT2 -

つくった

#include <Wire.h>
#include <Arduino.h>

// デバイスアドレス(スレーブ)
uint8_t DEVICE_ADDRESS = 0x53;  

// XYZレジスタ用のテーブル(6byte)
uint8_t RegTbl[6];  

void forward(uint32_t);
void reverse(uint32_t);
void getAccelerationData();

/* 使うピンの定義 */
const int IN1 = 25;
const int IN2 = 26;

/* チャンネルの定義 */
const int CHANNEL_0 = 0;
const int CHANNEL_1 = 1;

const int LEDC_TIMER_BIT = 8;   // PWMの範囲(8bitなら0〜255、10bitなら0〜1023)
const int LEDC_BASE_FREQ = 490; // 周波数(Hz)
const int VALUE_MAX = 255;      // PWMの最大値

void setup() {
  Serial.begin(9600); 

  // マスタとしてI2Cバスに接続する
  Wire.begin();       
  // DATA_FORMAT(データ形式の制御) 
  Wire.beginTransmission(DEVICE_ADDRESS);  
    // DATA_FORMATのアドレス    
  Wire.write(0x31);
    // 「最大分解能モード」 及び 「±16g」 (0x0B == 1011)
  Wire.write(0x0B);  
    // 「10bit固定分解能モード」 及び 「±16g」にする場合 (0x03 == 0011)
    // Wire.write(0x03);
  Wire.endTransmission();
  // POWER_TCL(節電機能の制御) 
  Wire.beginTransmission(DEVICE_ADDRESS);  
    // POWER_CTLのアドレス    
  Wire.write(0x2d);
    // 測定モードにする
  Wire.write(0x08);  
  Wire.endTransmission();

  pinMode(IN1, OUTPUT); // IN1
  pinMode(IN2, OUTPUT); // IN2

  // ピンのセットアップ
  ledcSetup(CHANNEL_0, LEDC_BASE_FREQ, LEDC_TIMER_BIT);
  ledcSetup(CHANNEL_1, LEDC_BASE_FREQ, LEDC_TIMER_BIT);

  // ピンのチャンネルをセット
  ledcAttachPin(IN1, CHANNEL_0);
  ledcAttachPin(IN2, CHANNEL_1);
}

void loop() {

  getAccelerationData();

  // データを各XYZの値に変換する(LSB単位)
  int16_t x = (((int16_t)RegTbl[1]) << 8) | RegTbl[0];
  int16_t y = (((int16_t)RegTbl[3]) << 8) | RegTbl[2];
  int16_t z = (((int16_t)RegTbl[5]) << 8) | RegTbl[4];  


  // 各XYZ軸の加速度(m/s^2)を出力する
  Serial.print("X : ");
  Serial.print( x * 0.0392266 );
  Serial.print(" Y : ");
  Serial.print( y * 0.0392266 );
  Serial.print(" Z : ");
  Serial.print( z * 0.0392266 );
  Serial.println(" m/s^2");

 //下降していたら
  if(x * 0.0392266 >= 9.1){
    forward(200);
 //上昇またはホバリング状態なら
  }else{
    forward(100);
  }

  delay(100);
}

//加速度データ取得
void getAccelerationData(){
  // XYZの先頭アドレスに移動する
  Wire.beginTransmission(DEVICE_ADDRESS);
    Wire.write(0x32);
  Wire.endTransmission();

  // デバイスへ6byteのレジスタデータを要求する
  Wire.requestFrom(DEVICE_ADDRESS, 6);

  // 6byteのデータを取得する
  int i;
  for (i=0; i< 6; i++){
    while (Wire.available() == 0 ){}
    RegTbl[i] = Wire.read();
  }

}

// 正転
void forward(uint32_t pwm) {
  if (pwm > VALUE_MAX) {
    pwm = VALUE_MAX;
  }
  ledcWrite(CHANNEL_0, 0);
  ledcWrite(CHANNEL_1, pwm);
}

// 逆転
void reverse(uint32_t pwm) {
  if (pwm > VALUE_MAX) {
    pwm = VALUE_MAX;
  }
  ledcWrite(CHANNEL_0, pwm);
  ledcWrite(CHANNEL_1, 0);
}

結果


値が取れた

モータが回った

とりあえずモータを加速度で制御して、浮き沈みしながらホバリングする機構はできた。
あとは電源。まだまだ飛べなかった。

参考

I2C通信の使い方
鍵かけ忘れ検知システムを作る
3軸加速度センサー(ADXL345)の使い方 - I2C版 [Arduino]
【ESP32】PWMでモーターを制御する方法

めっちゃ参考にしました、ありがとうございます。