【新型コロナ】手のひらでの顔触り防止ガジェット完成<TOFとIMU組合せ>

M5Stickとオプションだけで、顔、頭を手のひらで触る動作を検知して、警告音を鳴らすガジェット作れました。YOTUBEご覧ください

 

●構成と原理
M5StickCとオプションTOFユニット VL53L0XスピーカーHATだけで完成します。
switchsicienceさんが本体とスピーカーHAT品切れ状態ですが、
コロナ禍で中国からの入荷が滞っているみたいですが、私のは昨年仕入れたものです。

原理1:手の動きを内蔵IMUのpitch,roll角度を使って検出します。
原理2:手の姿勢がわかったら顔面からの距離をTOFユニットで測距して
ある一定距離になったら、スピーカーから警告音だします。

当初、IMUだけで実現しようとしたのですが、顔面との距離がわからないと無理なので
手ごろなTOF VL53L0Xを使ってみました。応答は50msecと遅いですが安くてそこそこの精度がでるので、人間の日常動作程度なら十分使えます。
使い方は、過去記事に3本あります。

【L-RTK】レーザーTOFセンサVL53L0Xで測距治具作った<速度遅い>

【L-RTK】レーザーTOF_VL53L0Xの精度測定<直線性OKヒス有>

M5StickのIMUは、ライブラリーが優秀です。  pitch,roll,yawが計算されて
でてきます。これは、カルマンフィルタなどフィルタを使わないと得られない姿勢パラメータなので、IMUライブラリに内蔵されていて、ユーザーが苦労して計算しないでもよくて、非常にべんりです。

原理3:閾値 pitch角度-20度以上、rollk角20度以上、距離200mm以内で顔の周囲に手のひらが近づいたと認識します。これは、私の右手に合わせた値なので、人によって合わせる作業が必要です。

●プログラム
 サンプルを組み合わせれば完成します。ArdunoIDEのM5StickCのTOFとIMUとSPEAKERの3つを合体すればいいです。

//the original code by Ted Meyers
//posted here: https://groups.google.com/d/msg/diyrovers/lc7NUZYuJOg/ICPrYNJGBgAJ
//#include <M5StickC.h>#include <M5StickC.h>
#include <Wire.h>#define VL53L0X_REG_IDENTIFICATION_MODEL_ID 0xc0
#define VL53L0X_REG_IDENTIFICATION_REVISION_ID 0xc2
#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD 0x50
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x70
#define VL53L0X_REG_SYSRANGE_START 0x00
#define VL53L0X_REG_RESULT_INTERRUPT_STATUS 0x13
#define VL53L0X_REG_RESULT_RANGE_STATUS 0x14

#define ToF_ADDR 0x29//the iic address of tof
//==============IMU==========================
float accX =0.0F ;
float accY =0.0F ;
float accZ =0.0F;

float gyroX = 0.0F;
float gyroY = 0.0F;
float gyroZ = 0.0F;

float pitch = 0.0F;
float roll = 0.0F;
float yaw = 0.0F;

//=============================================
int fcontact;
int i,j,k;
byte gbuf[16];
int distance;
int dis[10]={};
int disum=0;
float disave=0;
//——-Speaker———-
const int servo_pin = 26;
int freq = 50;
int ledChannel = 0;
int resolution = 10;
extern const unsigned char m5stack_startup_music[];

void setup() {
//========================IMU SETUP=============================
M5.IMU.Init();
M5.Lcd.setRotation(1);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(1);
M5.Lcd.setCursor(40, 0);
M5.Lcd.println(“IMU TEST”);
M5.Lcd.setCursor(0, 10);
M5.Lcd.println(” X Y Z”);
M5.Lcd.setCursor(0, 50);
M5.Lcd.println(” Pitch Roll Yaw”);

//============Speaker setup==============================
ledcSetup(ledChannel, freq, resolution);
ledcAttachPin(servo_pin, ledChannel);
ledcWrite(ledChannel, 256);//0°

// put your setup code here, to run once:
Wire.begin(); // join i2c bus (address optional for master)
Serial.begin(115200); // start serial for output
//Serial.println(“VLX53LOX test started.”);

//—osmar
M5.begin();

M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextColor(WHITE);
M5.Lcd.setTextSize(2);
//—osmar
M5.Lcd.setCursor(0, 0);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.print(“Distance: “);

}
void playMusic(const uint8_t* music_data, uint16_t sample_rate) {
uint32_t length = strlen((char*)music_data);
uint16_t delay_interval = ((uint32_t)1000000 / sample_rate);
for(int i = 0; i < length; i++) {
ledcWriteTone(ledChannel, music_data[i]*50);
delayMicroseconds(delay_interval);
}
}

void loop() {
i++;
//===============IMU =========================
M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
M5.IMU.getAccelData(&accX,&accY,&accZ);
M5.IMU.getAhrsData(&pitch,&roll,&yaw);

//============================================

distance=measure_distance();
dis[i%5]=distance;
for (j=0;j<5;j++)
{
disum=disum+dis[j];
}
disave=(float)disum/5;
disum=0;
distance=int(disave);
if(distance>500){distance=500;};
//Roll>=20deg && distance<=15cm =>FaceContact
if (pitch>-30 && roll>=20 && distance<200 )
{
fcontact=100;
ledcWriteTone(ledChannel, 1250);
//delay(1000);
}

else{
fcontact=0;
ledcWriteTone(ledChannel, 0);
}
//Serial.printf(“%d,%4.3f,%4.3f,%4.3f\r\n”, distance,accX,accY,accZ);
Serial.printf(“%d,%4.3f,%4.3f,%4.3f,%d\r\n”, distance,pitch,roll,yaw,fcontact);
delay(50);

if(M5.BtnA.wasPressed())
{
esp_restart();
}
M5.update();
}

int measure_distance() {
write_byte_data_at(VL53L0X_REG_SYSRANGE_START, 0x01);

read_block_data_at(VL53L0X_REG_RESULT_RANGE_STATUS, 12);//read 12 bytes once

uint16_t dist = makeuint16(gbuf[11], gbuf[10]);//split distance data to “dist”
byte DeviceRangeStatusInternal = ((gbuf[0] & 0x78) >> 3);

//Serial.print(“distance “);
// Serial.print(dist);
//Serial.print(“,”);
//Serial.println(millis());

M5.Lcd.setCursor(0, 25);
M5.Lcd.fillRect(0, 25, 100, 25, BLACK);
M5.Lcd.print(dist);
M5.Lcd.print(“mm”);
return dist;
}

uint16_t bswap(byte b[]) {
// Big Endian unsigned short to little endian unsigned short
uint16_t val = ((b[0] << 8) & b[1]);
return val;
}

uint16_t makeuint16(int lsb, int msb) {
return ((msb & 0xFF) << 8) | (lsb & 0xFF);
}

uint16_t VL53L0X_decode_vcsel_period(short vcsel_period_reg) {
// Converts the encoded VCSEL period register value into the real
// period in PLL clocks
uint16_t vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
return vcsel_period_pclks;
}

/*
* IIC Functions
*/
/* function description: write one byte data */
void write_byte_data(byte data) {
Wire.beginTransmission(ToF_ADDR);
Wire.write(data);
Wire.endTransmission();
}

/* function description: write one byte data to specifical register */
void write_byte_data_at(byte reg, byte data) {
Wire.beginTransmission(ToF_ADDR);
Wire.write(reg);
Wire.write(data);
Wire.endTransmission();
}

/* function description: read two bytes data to specifical register */
void write_word_data_at(byte reg, uint16_t data) {
byte b0 = (data &0xFF);
byte b1 = ((data >> 8) && 0xFF);

Wire.beginTransmission(ToF_ADDR);
Wire.write(reg);
Wire.write(b0);
Wire.write(b1);
Wire.endTransmission();
}

/* function description: read one byte data */
byte read_byte_data() {
Wire.requestFrom(ToF_ADDR, 1);
while (Wire.available() < 1) delay(1);
byte b = Wire.read();
return b;
}

/* function description: read one byte data from specifical register */
byte read_byte_data_at(byte reg) {
//write_byte_data((byte)0x00);
write_byte_data(reg);
Wire.requestFrom(ToF_ADDR, 1);
while (Wire.available() < 1) delay(1);
byte b = Wire.read();
return b;
}

/* function description: read two bytes data from specifical register */
uint16_t read_word_data_at(byte reg) {
write_byte_data(reg);
Wire.requestFrom(ToF_ADDR, 2);
while (Wire.available() < 2) delay(1);
gbuf[0] = Wire.read();
gbuf[1] = Wire.read();
return bswap(gbuf);
}

/* function description: read multiple bytes data from specifical register */
void read_block_data_at(byte reg, int sz) {
int i = 0;
write_byte_data(reg);
Wire.requestFrom(ToF_ADDR, sz);
for (i=0; i<sz; i++) {
while (Wire.available() < 1) delay(1);
gbuf[i] = Wire.read();
}
}

 

●以後
  BlueToothでスマホに飛ばして、音声で警告する仕様も面白いですが、YOUTUBEでの反響をみて以後いじるか決めます。
 関心があって、是非とも欲しい方がいらしたらフォローいたします。

 

コメントを残す

メールアドレスが公開されることはありません。 が付いている欄は必須項目です