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

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

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

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

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

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

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

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

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

 

■加速度センサADXL354BのAD変換プログラムを追加
M5Stack,M5Stickは、単独のMBUSだとAD変換ポートが2個しかありません。
3軸のセンサだと2軸しか使えませんが、今回は、測距離治具なので、Z方向は
データとして使わないので進行方向をX軸として振れ成分としてY軸の2CHで
使うことで妥協しました。事前に加速度2CHだけのテストプログラムで動作
確認しました。ポートは、26,36しか使えないので迷いません。

#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);
}


■TOFと加速度センサを合体したプログラム
CPLTで全データを表示させるように、loopでまとめてSerial.printlnしてます。
M5系のprint文は、Arduinoより進化していてC++そのままのフォーマットで
使えるので、行数が短くて見やすくなっていて具合いいです。
加速度センサのAD変換値は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();
}
}

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

 

●以後
 治具ができたので、キャリブレーションして、TOFの精度求めます。

●M5StickCのSPIFFSを使いたい場合
http://shinshu-makers.net/shinshu_makers/%e3%80%90%ef%bd%8d%ef%bc%95%e3%80%91m5stickc%e3%81%a7spiffs%e5%8b%95%e4%bd%9c%e5%ae%9f%e9%a8%93%ef%bc%9c%e6%9b%b8%e3%81%8d%e8%be%bc%e3%81%bf%e9%81%85%e3%81%8416byte-msec%ef%bc%9e/

コメントを残す

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