M5StackGrayレーザー距離計スキャン動作テスト

レーザ距離センサVL53L1XをRCサーボSG92Rに載せて，180度スキャンして，周囲の壁などの反射面までの距離を測定し，空間形状を把握します。
レーザ距離センサVL53L1XおよびRCサーボSG92RはM5StackGrayでコントロールします。

レーザ距離センサVL53L1XおよびRCサーボSG92R単体でのテストはそれぞれ次のリンクで行っています。

　レーザー距離センサVL53L1X　 ≫
　RCサーボSG92R　 ≫

配線は次のようになります。
レーザ距離センサVL53L1XおよびRCサーボSG92Rの配線は単体テストの時と同じです。
（GNDの共通化は，M5StackのGND端子で行っています。）

取得したデータはBluetoothでスマホなどに送信しています。そのため，Bluetoothライブラリも使用しています。
スマホではBlueoothTerminalのアプリを使います。
VL53L1Xの操作に関しては専用ライブラリを使用しています。
SG92Rの操作に関しては独自のライブラリを使用しています。

実行にあたってはM5StackのAボタンでスキャン開始，Bボタンでスキャン中止，Cボタンで初期位置復帰です。

　レーザー距離計スキャン動作テストメインプログラム　DistanceScanner.ino

//DistanceScanner.ino//

//レーザ距離計をRCサーボに載せて，180°スキャンして，それぞれの方向における周囲の壁までの距離を測定し，
//測定ごとにBT通信でAndroid端末などBT通信親機に送信する
//スキャンは1.8°ステップで行う。
//Aボタンで測定開始
//Bボタンで測定中止
//ボタンCで初期位置復帰

//RCServo PWM周期：20mS
//RCServo 制御パルス：0.5ms～2.4ms　（0-180°に対応？）

#include <M5Stack.h>
#include <Wire.h>
#include <VL53L1X.h>
#include <BluetoothSerial.h>
#include "RCServoDriver.h"

//M5Stack　定数
const int SDApin = 21; //SDAピン番号 LaserDistanceSensorで使用
const int SCLpin = 22; //SCLピン番号 LaserDistanceSensorで使用
const int I2C_frequency = 400000; //400 kHz I2C

//RCServo　定数
const uint8_t nBits_forRCS = 16; // RCServoに使用するビット数　n=1～16[bit]
const double RCS_Period = 20.0;   // RCServo周期[ms]
const double RCS_Frequency = 1000. / RCS_Period; //RCServo周波数[Hz]
const double minlength = 0.5; //[msec] 制御パルス長の最小値
const double maxlength =2.4; //[msec] 制御パルス長の最大値
const uint8_t RCS_CH = 8; // RCServo PWMチャンネル
const uint8_t RCS_PIN = 26; // RCServo出力に使用するGPIO PIN番号
const int nSteps = 100;

//LaserDistanceSensor定数
const unsigned long int MeasurementTimingBudget = 50000;
      //測定タイミングバジェット(1回の距離測定)に許容される時間[micros]
const unsigned int MeasuremenInterval = 50;
      //連続測定モードの測定間隔[msec]　0を指定すると可能な限り最速になる
const unsigned int LDS_Timeout = 500;
      //[msec] センサーの準備ができていない場合に読み取り操作中止 0 は設定しない

//M5Stack
boolean APressed = false;
boolean BPressed = false;
boolean CPressed = false;

//Bluetooth
BluetoothSerial SerialBT;
char buff[128];
int ichar=0;

//RCServo
RCServoDriver mRCServoDriver;
boolean scanning = false;
boolean waiting = false;
unsigned long time0;
int StepIndex = 0;
int previousStepIndex = 0;
double PulseLength;
double generatePulseLength(int StepIndex);
void gotoHome();

//LaserDistanceSensor
VL53L1X LaserDistSensor;

void setup() {
SerialBT.begin("M5Stack"); //Bluetooth device name
M5.begin();
M5.Power.begin();
M5.Lcd.setTextSize(2);
M5.Lcd.println("LaserDistSensor Test");
M5.Lcd.println("The device is ready");
M5.Lcd.println("A:Start B:Abort C:Home");

//M5Stack LaserDistanceSensorのためのI2C設定
Wire.begin(SDApin,SCLpin); //ここを指定
Wire.setClock(I2C_frequency);

///RCServo設定
mRCServoDriver.setPins(RCS_PIN, RCS_CH);
mRCServoDriver.setPWM(nBits_forRCS, RCS_Frequency);
mRCServoDriver.setMinMax(minlength, maxlength);

//LaserDistanceSensor設定
LaserDistSensor.setTimeout(LDS_Timeout);
if (!LaserDistSensor.init())
{
    M5.Lcd.println("Failed to detect and initialize LaserDistSensor!");
    while (1);
}
LaserDistSensor.setDistanceMode(VL53L1X::Long);
LaserDistSensor.setMeasurementTimingBudget(MeasurementTimingBudget);
LaserDistSensor.startContinuous(MeasuremenInterval);
}

void loop() {
    if (SerialBT.available()) {
        char c=(char)SerialBT.read();
        switch (c) {
         case '\n':
            break;
         case '\r':
            buff[ichar]=0;
            M5.Lcd.println(buff);
            ichar=0;
            break;
         default:
            buff[ichar++]=c;
            if (ichar==127) {
              buff[ichar]=0;
              M5.Lcd.println(buff);
              ichar=0;
            }
            break;
        }
    }
    M5.update();   // update button state
    if (M5.BtnA.pressedFor(50)) APressed = true;
    if (M5.BtnB.pressedFor(50)) BPressed = true;
    if (M5.BtnC.pressedFor(50)) CPressed = true;
    if(APressed && M5.BtnA.wasReleased()) {
         SerialBT.println("Started");
         M5.Speaker.tone(440, 200); //Peett
         APressed = false;
         if (previousStepIndex != 0) gotoHome();
         StepIndex = previousStepIndex = 0;
         scanning = true;
    }
    if(BPressed && M5.BtnB.wasReleased()) {
         SerialBT.println("Aborted");
         M5.Speaker.tone(880, 200); //Peett
         CPressed = false;
         scanning = waiting = false;
    }
    if(CPressed && M5.BtnC.wasReleased()) {
         SerialBT.println("Home");
         M5.Speaker.tone(1760, 200); //Peett
         CPressed = false;
         scanning = waiting = false;
         gotoHome();
    }
    if (scanning) {
      if (waiting) {
        M5.Lcd.setCursor(0, 60);
        M5.Lcd.printf("%5ld",millis() - time0);

        if (400 < millis() - time0) {
          waiting = false;
          int result = LaserDistSensor.read();
          if (LaserDistSensor.timeoutOccurred()) {
            M5.Lcd.setCursor(0, 80);
            M5.Lcd.println("LDS Timeout");
            SerialBT.println("LDS Timeout");
            scanning = false;
          } else {
            const char *statusstr = VL53L1X::rangeStatusToString(LaserDistSensor.ranging_data.range_status);
            M5.Lcd.setCursor(0, 80);
            M5.Lcd.printf("%d, %f\n", StepIndex, PulseLength);
            M5.Lcd.printf("%6d\n", result);
            M5.Lcd.printf("status: %-20s\n", statusstr);
            SerialBT.printf("%d, %.3f, %d, %s\r\n", StepIndex, PulseLength, result, statusstr);
            previousStepIndex = StepIndex++;
            if (nSteps < StepIndex) scanning = false;
          }
        }
      } else {
        PulseLength = generatePulseLength(StepIndex);
        mRCServoDriver.driveRCServo(PulseLength);
        time0 = millis();
        waiting = true;
      }
    }
}

void gotoHome() {
mRCServoDriver.driveRCServo(generatePulseLength(0));
StepIndex = previousStepIndex = 0.;
delay(500);//[msec]
}

double generatePulseLength(int StepIndex) {
double Length = (maxlength - minlength)/ nSteps * StepIndex + minlength;
return Length;
}

//RCServoDriver.h//

#ifndef RCServoDriver_h
#define RCServoDriver_h

class RCServoDriver {
public:
    void setPins(int RCServoPin, int RCServoChannel);
    void setPWM(int nBits_forRCS, double RCS_Frequency);
        //RCS_Frequency [Hz]
    void setMinMax(double minlength, double maxlength);
        //minlength, maxlength [msec]
    int driveRCServo(double PulseLength);
        // PulseLength [msec]
private:
    int pin;
    int channel;
    double period; //[msec]
    int pwmMaxValue;
    double minpulse, maxpulse; //[msec]
};
#endif

//RCServoDriver.cpp//

#include <Arduino.h>
#include "RCServoDriver.h"

void RCServoDriver::setPins(int RCServoPin, int RCServoChannel) {
pin = RCServoPin;
channel = RCServoChannel;
pinMode(pin, OUTPUT); //出力したいピンを出力用途に設定
}

void RCServoDriver::setPWM(int nBits_forRCS, double RCS_Frequency) {
//RCS_Frequency [Hz]
pwmMaxValue = 1<<nBits_forRCS;
period = 1000. / RCS_Frequency; //[msec]
// 指定PWMチャンネルにPWM周波数と使用ビット数を設定
ledcSetup(channel, RCS_Frequency, nBits_forRCS);
// 出力したいピンにPWMチャンネル出力をつなげる設定
ledcAttachPin(pin, channel);
}

void RCServoDriver::setMinMax(double minlength, double maxlength) {
minpulse = minlength;
maxpulse = maxlength;
}

int RCServoDriver::driveRCServo(double PulseLength) {
// PulseLength [msec]
if (PulseLength < minpulse) PulseLength = minpulse;
if (maxpulse < PulseLength) PulseLength = maxpulse;
int duty = pwmMaxValue * PulseLength / period;
//RCサーボを駆動するパルス長を与える
ledcWrite(channel, duty);
return duty; //デバッグ用 dutyratio = duty / pwmMaxValue
}

テストは次のような場所（壁のある部屋）で行いました。床はフローリングで，壁にはエンボスのある壁紙が貼ってあります。

測定の様子(一部)　RCサーボに載ったレーザ距離計がスキャンしています。

　得られたCSVファイル

Started
0, 0.500, 1513, wrap target fail
1, 0.519, 1491, range valid
2, 0.538, 1492, range valid
3, 0.557, 1510, range valid
4, 0.576, 1524, range valid
5, 0.595, 1531, range valid
6, 0.614, 1530, range valid
7, 0.633, 1544, range valid
8, 0.652, 1549, range valid
9, 0.671, 1554, range valid
10, 0.690, 1549, range valid
11, 0.709, 1574, range valid
12, 0.728, 1578, range valid
13, 0.747, 1557, range valid
14, 0.766, 1559, range valid
15, 0.785, 1497, range valid
16, 0.804, 1496, range valid
17, 0.823, 1419, range valid
18, 0.842, 1319, range valid
19, 0.861, 1196, range valid
20, 0.880, 1107, range valid
21, 0.899, 1053, range valid
22, 0.918, 966, range valid
23, 0.937, 904, range valid
24, 0.956, 890, range valid
25, 0.975, 840, range valid
26, 0.994, 814, range valid
27, 1.013, 783, range valid
28, 1.032, 769, range valid
29, 1.051, 730, range valid
30, 1.070, 716, range valid
31, 1.089, 694, range valid
32, 1.108, 679, range valid
33, 1.127, 658, range valid
34, 1.146, 646, range valid
35, 1.165, 634, range valid
36, 1.184, 615, range valid
37, 1.203, 608, range valid
38, 1.222, 592, range valid
39, 1.241, 582, range valid
40, 1.260, 574, range valid
41, 1.279, 560, range valid
42, 1.298, 555, range valid
43, 1.317, 545, range valid
44, 1.336, 537, range valid
45, 1.355, 540, range valid
46, 1.374, 528, range valid
47, 1.393, 529, range valid
48, 1.412, 521, range valid
49, 1.431, 518, range valid
50, 1.450, 517, range valid
51, 1.469, 512, range valid
52, 1.488, 511, range valid
53, 1.507, 512, range valid
54, 1.526, 514, range valid
55, 1.545, 511, range valid
56, 1.564, 513, range valid
57, 1.583, 517, range valid
58, 1.602, 518, range valid
59, 1.621, 521, range valid
60, 1.640, 525, range valid
61, 1.659, 529, range valid
62, 1.678, 530, range valid
63, 1.697, 535, range valid
64, 1.716, 540, range valid
65, 1.735, 547, range valid
66, 1.754, 556, range valid
67, 1.773, 559, range valid
68, 1.792, 569, range valid
69, 1.811, 584, range valid
70, 1.830, 598, range valid
71, 1.849, 608, range valid
72, 1.868, 615, range valid
73, 1.887, 622, range valid
74, 1.906, 644, range valid
75, 1.925, 658, range valid
76, 1.944, 675, range valid
77, 1.963, 686, range valid
78, 1.982, 712, range valid
79, 2.001, 736, range valid
80, 2.020, 752, range valid
81, 2.039, 778, range valid
82, 2.058, 801, range valid
83, 2.077, 830, range valid
84, 2.096, 862, range valid
85, 2.115, 880, range valid
86, 2.134, 935, range valid
87, 2.153, 980, range valid
88, 2.172, 1038, range valid
89, 2.191, 1102, range valid
90, 2.210, 1146, range valid
91, 2.229, 1238, range valid
92, 2.248, 1285, range valid
93, 2.267, 1367, range valid
94, 2.286, 1403, range valid
95, 2.305, 1435, range valid
96, 2.324, 1420, range valid
97, 2.343, 1425, range valid
98, 2.362, 1426, range valid
99, 2.381, 1415, range valid
100, 2.400, 1405, range valid

この測定は42秒を要しました。このデータは0°から180°までレーザ距離計を振った時の，壁までの距離を表しています。
しかし，RCサーボの角度が90度のとき，レーザ距離計の測定方向は90度ではなく98度ほどを向いていることがわかりました。
そのため，測定値の検証ではこのずれを補正しています。実際に使用する場合も，このずれをなくすことは難しいので，事前にずれを測定しておき，測定値を補正する必要があります。
補正して，測定点を平面上にプロットして次の結果を得ました。2回分の測定結果を載せます。
部屋の角の部分は，正しく読み取れていないようです。壁へのレーザ光入射角が大きくなることが原因のようです。
2回とも同じようなデータが得られました。x方向の壁までの距離が1540程度，-x方向の壁までの距離が1420， y方向の壁までの距離が510程度と読み取れています。
RCサーボの回転中心と，距離計の中心のずれも影響しますが，20mm程度の誤差で読み取れているようです。（距離計単体は4m以下の誤差で読み取れることになっています。）

測定がうまくいっているのは床が光沢のあるフローリングであり，センサ側に不要な反射が少ないこと，壁がエンボス加工の壁紙であり適度に反射することの影響があると思います。

レーザ距離センサVL53L1XをRCサーボSG92Rに載せて，180度スキャンして，周囲の壁などの反射面までの距離を測定しました。角度に関しては組立時の誤差があり8度ほど傾いた値が得られましたが，これを補整することで，20mm程度の誤差内で測定ができました。この測定は42秒を要しました。主にはRCサーボの間欠的な回転の静定まで待ち時間（0.4秒/測定，40秒/100測定）を入れていることに起因します。

M5StackGrayでレーザー距離計スキャン動作テスト