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

M5Stack用ＴＯＦセンサユニットを使って加速度センサ距離測定用治具作りました。

●M5Stack用ＴＯＦセンサ　VL53L0X
このチップは秋月でも販売してますが、M5Stack用のパッケージにはいったほうが
取り回しが楽そうなので1年前に仕入れておきました。
https://www.switch-science.com/catalog/5219/

M5Stack用は、Grooveコネクタで接続できるようになっているので、M5Stickでも使えます
今回は、大きな液晶ＤＩＳＰＬＡＹは使わないので、M5Stickを使ってみました。
加速度センサＡＤＸＬ３５４Ｂは、NucleoL432KCに接続して使ってきたのですが
VL53L0X用のライブラリーがシンプルで判りやすいのが無かったので、M5Stickで
加速センサ2回積分校正用測距計測システムを組むことにしました。

①M5Stack用ＴＯＦセンサの試運転
■チュートリアルページがここにあります。https://docs.m5stack.com/#/en/unit/tof

■VL53L0X　チップの仕様書はこちらです。
速度が20-30msecサンプリングで３－５％精度で２ｍ範囲だそうです。
加速度センサの積分距離比較用なのでこの程度の精度でもよいかと思います。
加速センサ2回積分した距離の精度は１０％でれば御の字と覚悟してます。
https://m5stack.oss-cn-shenzhen.aliyuncs.com/resource/docs/datasheet/hat/VL53L0X_en.pdf

■ArduinoＩＤＥの環境設定
Arduino IDEで、M5StackとM5Stickを動作させる環境を作っておきます。
UIFLOWなど使えますが、汎用性のあるArduinoIDEのＣ＋＋を使います。
これはM5Stack社サイトに丁寧に解説があるで安心です。
https://docs.m5stack.com/#/en/arduino/arduino_development

■ＴＯＦセンササンプルプログラム
ＧＩＴにおいてあるのですが、コピペします。
https://github.com/m5stack/M5-ProductExampleCodes/blob/master/Unit/TOF/Arduino/MeasureDistance/MeasureDistance.ino
ここで使っているのは、<M5Stack.h>と<Wire.h>です。
M5Stickに代えるのは、<M5StickC.h>に書き換えて、
Tool ボードをM5StickCに代えるだけです。
コンパイルしたらM5Stickの小さな液晶ＤＩＳＰＬＡＹでも距離数値が表示されました。
仕様書通り20msecDelayいれるとフリーズしてしまったので、50msecで何とか安定しました。スピードと精度が反比例しているので、あまり速い現象は不得意です。

※ArduinoIDEのファイルースケッチ例ーM5stcikC-UNIT-TOFVL53L0Xにもサンプル
プログラムがあるのですが、これが複雑で判りにくいので私はM5Stack用を改造しました。

■加速度センサADXL354BのＡＤ変換プログラムを追加
M5Stack,M5Stickは、単独のＭＢＵＳだとAD変換ポートが2個しかありません。
3軸のセンサだと2軸しか使えませんが、今回は、測距離治具なので、Ｚ方向は
データとして使わないので進行方向をＸ軸として振れ成分としてＹ軸の２ＣＨで
使うことで妥協しました。事前に加速度２ＣＨだけのテストプログラムで動作
確認しました。ポートは、２６，３６しか使えないので迷いません。

#include <M5StickC.h>

int PINX = 26;
int PINY = 36;
void setup() {
M5.begin();

pinMode(PINX, ANALOG);
pinMode(PINY, ANALOG);
}

void loop() {
Serial.printf(“%04d,%04d\r\n”, analogRead(PINX),analogRead(PINY));
delay(5);
}

■ＴＯＦと加速度センサを合体したプログラム
ＣＰＬＴで全データを表示させるように、loopでまとめてSerial.printlnしてます。
M5系のprint文は、Arduinoより進化していてＣ＋＋そのままのフォーマットで
使えるので、行数が短くて見やすくなっていて具合いいです。
加速度センサのＡＤ変換値は10回の移動平均をいれてノイズとってます。
測距関数 void measeure_distance()を int measure_distance(){・・・return dist}
inoファイルのzipです。
TOF_VL53L0X_M5SickC_AccXYrev01.ino

//the original code by Ted Meyers
//posted here: https://groups.google.com/d/msg/diyrovers/lc7NUZYuJOg/ICPrYNJGBgAJ
#include <M5StickC.h>//#include <M5Stack.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
int i;
byte gbuf[16];
int distance;
int t_1,t;
//——-ADXL set———-
int PINX = 26;
int PINY = 36;
int AX[10]={};
int AY[10]={};
int AXave;
int AYave;
void setup() {
// 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();
pinMode(PINX, ANALOG);
pinMode(PINY, ANALOG);
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 loop() {
//Serial.println(“—– START TEST —-“);
distance=measure_distance();
for (i=0;i<10;i++)
{
AX[i]=int(analogRead(PINX));
AY[i]=int(analogRead(PINY));
//Moving Average 10
AXave=int((AX[0]+AX[1]+AX[2]+AX[3]+AX[4]+AX[5]+AX[6]+AX[7]+AX[8]+AX[9])/10);
AYave=int((AY[0]+AY[1]+AY[2]+AY[3]+AY[4]+AY[5]+AY[6]+AY[7]+AY[8]+AY[9])/10);
t_1=t;
t=millis();
Serial.printf(“%d,%d,%d,%d\r\n”, distance,AXave,AYave,t-t_1);
delay(5);
}
if(M5.BtnB.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 onceuint16_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();
}
}

■データ確認
ターゲットは黒でマットを使ったら全然精度がでませんでした。真っ白な段ボールが
一番きれいで精度よくでました。