🦊

地下壕・建物の断面が測定できるポーダブルサイズのLIDAR「手のひらLIDAR ver2」の製作

2023/08/16に公開

m5stickc

地下壕や部屋などの断面形状を測定する携帯型LIDAR（手のひらLIDAR）について書いてゆきます。前回の手のひらLIDAR ver1ではゲジゲジ付きの土の壁には対抗できなかったので、今回はレーザー距離計を使って測定能力を上げました。

記載されてる技術

レーザー距離計（LIDAR）ユニットの使い方（怪しい中国製部品の使いこなし）
サーボモーターの精密制御
ESP32（M5StickC）での制御
HTTPを使ってスマホからESP32（M5StickC）をコントロール

1.初めに

地下壕などを探検してると、穴の大きさや断面形状が知りたくなります。いやもうなるんです。なるの前提で話ししましょう。そんな時に便利な携帯型LIDAR ver1を中国製ロボット掃除機のLIDARモジュールを使って作ってみました。しかしこのLIDAR、コンクリートの壁はばっちりなんですが、土の壁はレーザーが乱反射されて1mも離れると距離が測定できない・・・。「これは使えん」ということで、今回は40mまで測定できるとの触れ込みのレーザー距離計モジュールを使って再チャレンジしてみました。このモジュールのおかげで土壁だろうとゲジゲジ様付きの壁だろうが測定できるようになりました！ただ、スキャンに使ったサーボモーターの精度が正直イマイチ。まあ、「地下壕の断面形状を測定する」という目的には十分なんですが、技術屋としてはモヤモヤがくすぶる結果になってしまいました。やっぱりラジコン用のサーボじゃだめで、ステッピングモーター使わなあかんか・・・。

2. 全体概要

全体は下のような感じでまとめました。

フタを開けて測定可能状態にしたとき
壁に向かって照射中
フタを閉めたとき
ホームセンターで買ってきたプラスチックケースに、

レーザー距離計モジュール
スキャン用のサーボモーター
制御用コンピューター（M5StickC）
電源ON/OFF用のスイッチ
モバイルバッテリー

を詰め込みました。フタを開けて90°に固定すると、垂直方向に距離スキャンができるようになります。このフタを90°に固定するための金具、みんな大好き閉り止めです。100均で売るべきだ！

コントロールはM5StickCをWiFiアクセスポイントにして、スマホからWiFiで接続してHTMLでやります。スマホでスキャンボタンを押すと、0°～180°までスキャンして結果を表示します。
スマホのコントロール画面
Scan0は通常のレーザー距離計のように手で持って狙ったところまでの距離を連続測定します。M5StickCの慣性センサーを使って傾斜角度を測定し、そこから水平距離、垂直距離測定もできるようにしました。ANGはレーザーの水平面からの角度、DSTは目標までの水平距離、HETは目標の高さです。

回路構成はこんな感じです。

3. 使った部品について

レーザー距離計モジュール

AliExpressで「レーザー距離計　モジュール」で検索すると出てきますが、数は少ないです。最低でも4000円ぐらい、高いのだと数万円もありますが・・・もちろん4000円のを選びました。どっちにせよ得体が知れない品だし。私が入手したのはこれ↓
TW10S-UART

選ぶ時のキーポイントは、

測定速度
端子配置や制御コマンドが公開されてるか

ですね。LIDARはスキャンする＝多数の点を取るということで、とにかく測定速度が速いヤツがいいです。1点測るのに2秒かかってたら、180°を1°ステップで測定するのに360秒もかかります。そんなに長時間とどまってたらゲジゲジ様に何されるかわかんがな。上記TW-10S-UARTは測定速度「最大5Hz」とありまあまずまずです（後述するようにちょっと怪しいですが・・・）。制御方法についてもその他Webページにもちらほら関連情報があり、「何とかなりそう」感があります（ならんかったけど；後述）。
ところで、レーザー距離計の完成品を調べると1000円ぐらいからあります。3000円ぐらい出せば結構大きな液晶がついた立派なの買えます。中身のレーザー距離計モジュールが最低でも4000円なのになぜ？？完成品からレーザー距離計だけ取り出して使えればいいんだけど・・・カスタマイズされてたら手が出ないし・・・どなたかチャレンジしてみて下さい。

サーボモーター

レーザー距離計モジュールをスキャンするのに使うので180°回るタイプのが必要です。レーザー距離計モジュールは軽いのでラジコン用の小さなやつで十分です。
マイクロサーボ SG92R
https://www.amazon.co.jp/dp/B00WQWZXKW
が、問題は角度精度です。1°ステップぐらいでスキャンしたいんですが、ラジコン用のサーボでは後述のように±1°ぐらいのふらつきと、±4°ぐらいの系統誤差があります！この系統誤差は何とか補正可能ですが、ふらつきのほうはもうお手上げです（後述）。最初は安いパチモンを使ってたのでそのせいかと思ったのですが、サーボメーカーTowerPro社正規品でも同じようなレベルでした。この辺がラジコン用サーボの限界なのかも。
もっと精度を上げる方法として、ステッピングモーターを使うことも考えましたが、こいつら、制御に4本の信号線が必要な上、0点を検出する機構も必要です（サーボは絶対角度で制御できますが、ステッピングモーターは「今の位置から+1ステップ、-1ステップ」という制御しかできない）。ここまでで最低5本のGPIOが必要、ってM5StickCだとGPIOが足らんがな。んじゃ、もう普通のESP32使って、ってディスプレイと慣性センサーを別途準備せなあかんがな。ということで無間地獄に落ちそうだったのでまずはサーボモーターで我慢。

制御用コンピューター（M5StickC）

LIDAR ver1と同じくM5StickCを使いました。液晶がついてて測定値を表示できるのと、慣性センサーがついてるので傾きが検出できるのが〇です。別に普通のESP32でもいいですが、ディスプレイ＋慣性センサーを準備する必要があります。WiFiアクセスポイント機能とマルチタスクを使ってるのでArduinoではちょっと処理速度・メモリー不足と思います。

モバイルバッテリー

そんなにパワー使わないので小さいのでも十分です。100均物でも全然OK。なんだったらM5StickCの内蔵バッテリーでも動きました。この内蔵バッテリーのせいで逆に電源OFFがめんどくさいので、今回はスイッチをOFFにするとそれを検出してM5StickCがシャットダウンするというプログラムにしました。

閉り止め（ステー）

1個しか買ってないから、LIDAR ver1のやつを流用しました。なんでホームセンターとか100均に売ってないんだろう・・・
https://www.amazon.co.jp/dp/B00DLVCRMY

4. レーザー距離計モジュールの制御

前述したように、レーザー距離計モジュールTW10S-UARTは商品ページに少し説明がありますし、その他Webページにもちらほら関連情報があります。接続はUARTで、Rxでコマンドを受け、Txで結果を返す形で制御します。しかし、これだけじゃ何ともなりませんわです。後で知ったんですがArduinoのフォーラムでも議論されてて、1ショットの測定はできるようですが連続測定はできずじまいのようです。このフォーラムでTW10Sの詳しいコマンド説明書へのリンクがありますが、説明書はTW10Sで、一般的に売ってるのはTW10S-UART、私が入手したのもTW10S-UART・・・。どうもTW10Sをベースに、UARTで接続するようにカスタマイズした一品のようです。で、そのUARTのコマンド（MODBUS RTU）は説明書にはろくに載ってない。小さい字で「Note: MODBUS RTU communication protocol details, please contact us」とあります。「us」って誰やねん？！

レーザー距離計モジュールTW10S-UART制御までの道のり

もうしょうがないから販売元に聞いたよ。英語でメール書きました。そうしたらちゃんとWindowsで制御するためのソフトを送ってくれました（「TW10Sp--DLAK_.exe」ファイル名からして怪しすぎ）。まあ販売元がくれたやつだからとこわごわ立ち上げてみたら・・・ユーザーインターフェース中国語やん・・・うーんどうしよう。

しかし、気づきました。ボタンにポインターを当てると英語で説明が出る！何とかなりそうだ。
で、取りあえずTW10S-UARTをUSB-シリアル変換アダプターでPCに接続し、ソフトを動かしてみました。結果「ちゃんと動くやん！」。説明書には載ってない連続測定とか通信ボーレート変更とか、いろいろできる。なら、このソフトから出てる送信データを解析すれば一通りのコマンドがわかる！
ということで、USB-シリアル変換アダプターを2つ準備し、一つはデータ送信、もう一つでそのデータを受信という風にPCに接続し、ソフトから出てるコマンドをのぞき見しました。で、今度はターミナルソフトでそのコマンドをTW10S-UARTに送って、返ってくる応答を見ました。で、分かったコマンドが下表のとおり。この情報、（中国を除いた）世界初です。あー、Arduinoのフォーラムの人たちに教えてあげたい・・・

レーザー距離計モジュールTW10S-UARTのコマンド

まずTW10S-UARTに電源を供給すると立ち上げシーケンスに入り、正常に立ち上がると「OKっすよ」というメッセージとして
　01 03 02 00 00 B8 44
を送ってきます。以下のコマンドや応答も同じですが、先頭のバイトがモジュールのスレーブアドレス、その後がコマンド、太字がデータ、最後の2バイトがCRCコードというエラーチェック用のコードです。

コマンド内容	コマンド	応答
単発測定	01 03 00 0F 00 02 F4 08	01 03 04 xx xx xx xx yy yy 　xxが距離データ　yyがCRCコード
連続測定	01 03 00 01 00 02 95 CB	01 03 04 xx xx xx xx yy yy 　xxが距離データ　yyがCRCコード
連続測定停止	01 03 00 0A 00 02 E4 09
レーザーON/OFF	01 10 00 03 00 01 02 00 01 67 A3	OKなら　01 10 00 03 00 01 F1 C9
UARTボーレート変更（デフォルトは9600bps）	9600bps：　01 10 00 00 00 02 04 　　　　　00 00 25 80 E8 9F 38400bps：　01 10 00 00 00 02 04 　　　　　00 00 96 00 9C 0F 115200bps：　01 10 00 00 00 02 04 　　　　　00 01 C2 00 F3 0F	OKなら　01 10 00 00 00 02 41 C8
オフセット長確認	01 03 00 02 00 01 25 CA	0mm 　01 03 02 00 00 B8 44
オフセット長設定（デフォルトは0mm）	0mm：　01 10 00 04 00 01 02 00 00 A7 D4 10mm 　01 10 00 04 00 01 02 00 0A 27 D3	-
基準点確認	01 03 00 0A 00 02 E4 09	前基準：　01 03 02 00 01 79 84 後基準：　01 03 02 00 00 B8 44
基準点設定（デフォルトは前基準）	前基準：　01 10 00 0B 00 01 02 00 01 66 EB 後基準：　01 10 00 0B 00 01 02 00 00 A7 2B
スレーブアドレス確認	01 03 00 0C 00 01 44 09	アドレス1：　01 03 02 00 01 79 84
スレーブアドレス設定（デフォルトは1）	アドレス0：　01 10 00 0D 00 01 02 00 00 A7 4D アドレス1：　01 10 00 0D 00 01 02 00 01 66 8D アドレス10：　01 10 00 0D 00 01 02 00 0A 27 4A

例えば連続測定01 03 00 01 00 02 95 CBなら、最初の01がスレーブアドレス、次の03 00 01 00 02が「連続測定を開始せよ」というコマンド、最後の95 CBがCRCコードです。TW10S-UART側に01 03 00 01 00 02 95 CBを送信すれば連続測定が始まります。そうするとTW10S-UARTからは距離測定結果01 03 04 xx xx xx xx yy yyが鬼のように吐き出されてきますから、これを受信してxxのところを読めば距離がわかります。01 03 00 0A 00 02 E4 09を送信すればTW10S-UARTは連続測定をやめて待機状態になります。

コマンドのデータ値を変える場合の注意点（CRCコードについて）

ボーレートやオフセット長など、こちらからTW10S-UARTにデータを送信する場合は送るデータに合わせて最後の2バイト（CRCコード）を変更する必要があります（上表のコマンドをそのまま使う場合はコピペでいいです）。例えば「ボーレートを9600bpsに設定」の場合は01 10 00 00 00 02 04 00 00 25 80 E8 9Fですが、01 10 00 00 00 02 04までがコマンド、00 00 25 80が9600bps（16進数25 80 = 10進数9600）、E8 9FがCRCコードです。CRCコードはそれ以前までのデータから計算されるので、9600bpsのところを変更するとCRCコードも変わります。
例えばボーレートを4800bpsに変更する場合は、4800を16進数に直すと12C0なので、送るべきコマンドは01 10 00 00 00 02 04 00 00 12 C0になり、これにCRCコードを追加します。CRCコードの求め方はTW10S-UARTの説明書の最後の方に（なぜかこれだけは）詳しく書いてあるので、それ参照ですが、計算してくれるサイトもあります。例によって中国語ですが、入力欄に01 10 00 00 00 02 04 00 00 12 C0を入れて、「参数模型 NAME」でCRC-16/MODBUSを選択し、「计算」ボタンを押すと計算してくれます。結果は5FFFで、これを逆にしてFF 5FがCRCコードです。結果、送るべきコマンドは01 10 00 00 00 02 04 00 00 12 C0 FF 5Fになります。めんどくさ・・・

ところでこのTW10S-UARTですが、説明書には5Hz（=200msec/回）で測定できると書いてます。実際に連続測定モードにすると200msecごとぐらいにデータが送られてくるんですが、どうも4回同じデータが送られてくるような気配があります。測定中はレーザー1秒弱に1回ぐらい「ぴかぴかっ」と瞬くんですが、この瞬きの時に測定してるとすると、1秒弱に1回ぐらいちゃんと測定してあとは同じデータを200msecごとに送ってきてるんじゃ・・・という疑念がわいてきます。この疑惑があるため、後述のプログラムでは、測定時に300msecぐらい待って、一旦測定データを破棄してから新たなデータを受信するような制御にしてます。

5. サーボの制御

サーボの普通の制御方法はあちこちに解説がありますので省略します。+5VとGNDを電源につないで、Controlに制御用コンピューターからPWM信号を入れるだけです。しかし、今回はLIDARに使おうってことで角度の精度が欲しいです！ということで、そこの説明をメインに。

角度のふらつきについて

前述したように、ラジコン用サーボは角度精度に問題があります。ものの本には「サーボの精度はデッドバンド幅（Dead band width）という値と制御信号のPWM精度で決まる」とありますが、それなら使ったSG92RやよくあるSG90も仕様上は0.2°ぐらいの精度で動くはずです。実際「0.2°動かせ」という制御をすれば確かにサーボは何がしか動きます。でも動くだけで、0.2°の精度は出ません。ある時は1°ぐらい動くし、ある時は0.1°しか動かない。そんな感じのふらふらした動作になります。実際にレーザー距離計モジュールをサーボにつけて1°ステップでスキャンさせてレーザーの軌跡を見るとこんな感じの動きをします。

このふらつきランダムではなく、必ず同じ角度で同じようにふらつきます。これはサーボに使ってるエンコーダーの精度のせいだと思ってるんですが、どうでしょう。誰か詳しい人教えて下され。高級サーボモーターだと精度出るんかいな？？まあ、ラジコンに1°単位の精度いらんしな・・・ちなみにPWM信号の精度はオシロスコープでちゃんと精度よく出てることは確認済みです。

系統誤差について

んじゃ角度の絶対値はどうなんだというと、これがまた怪しそうです。下図はいったん組み上げたLIDARで部屋の一部の断面形状をスキャンしたものです。

いやいやいやいや、部屋の断面って普通長方形やろ。確かに庶民にやさしい系の価格の家だけどこれはない・・・　本来の部屋の形は下図の赤線のような感じのはずです。

で、この誤差原因なんですが、①LIDARの測定精度（レーザーが壁に斜めに当たると誤差が出る？）、②サーボの角度精度（0°、90°、180°はあってるけど、その間の角度がおかしい？）のどちらかです。で、状況から②が原因っぽいです。つまり、制御用コンピューターからは「45°にしろ」という指令を出してるですが、実際のサーボは40°になってる。んで、レーザー距離計からの距離測定データを制御用コンピューターは45°のデータとして計算処理するのでおかしなことになってしまう。こんな感じ↓

誤差の補正

ではどのぐらいの誤差があるのかなんですが、サーボの実際の角度を測って設定との差を見て・・・って、自分で1°単位の角度なんか測れるかい！　いうことで別の方法を使いました。

まず自分の部屋の断面形状がちゃんと長方形だと信じる（〇〇ホームの大工さんを信じる！）
左壁、天井、右壁までの距離は0°、90°、180°のデータで合ってると信じる（というか、0°、180°がちゃんと0°、180°になるようにPWM信号のデューティーを補正してるので合ってるはず）。
0°、90°、180°のデータから描ける部屋の形（長方形）とLIDARで測定した部屋の形との差を算出する
その差が角度誤差のせいだと仮定し、実際にレーザーが出てる角度を推定する↓

いや、もう、すいません。ごめん。これ読んでる人なら理解してくれることを祈るよ。
んで、下図が、設定角度と実際にレーザーが出てる角度の推定値との差です。

0°と90°と180°のデータは正しいとしてるので差は0です。グラフの細かいふらつきが最初に説明した「角度のふらつき」で、大きな波が「系統誤差」です。この角度のふらつきを補正するのはちょっと難しそうですが、系統誤差は3次か5次の関数になってそうなので式で補正できそうです。今回は3次式でフィッティングしました（グラフの赤線）。このフィッティング係数を使って設定角度を補正することでサーボを正確な角度で制御できそうです。
って、普通の人できるんかいな・・・いや、もう、ごめん。Excelでできるから。きっとできるから。
元のデータをこの方法で補正した結果が↓です。よかった。ちゃんと長方形だった。

コード内では以下のように設定したい角度servo_angleから補正角度（実際にサーボに与える角度）comp_angleを算出しています。そのあとservo_valueに与えるPWM値を計算してます。

/// 角度補正係数（3次関数フィッティング係数）
float ANGLE_A1 = 1.63E-05;
float ANGLE_A2 = -4.49E-03;
float ANGLE_A3 = 2.83E-01;
float ANGLE_A4 = -2.32E-01;

int servoMove(float servo_angle)
{
  // mapping angle to servo
  int servo_value;
  int result = true;
  float comp_angle;

  comp_angle = servo_angle+ANGLE_A1*pow(servo_angle,3)+ANGLE_A2*pow(servo_angle,2)+ANGLE_A3*servo_angle +ANGLE_A4;
  servo_value = map(int((comp_angle - ANGLE_OFFSET) * 10), 0, 1800, SERVO_000 * 10, SERVO_180 * 10) / 10;
  ledcWrite(CH_SERVO, servo_value);
  return;
}

6. LIDARの制御

スマホでの制御

コントロール、測定データ表示、データ保存はスマホでやります。M5StickCをWiFiアクセスポイントにして、スマホをWiFi接続し、HTMLでコントロールします。
HTMLでグラフを表示させる方法（リンク）
WebサーバーはLIDAR ver1では「WiFiServer」を使いましたが、今回は「WebServer」にしました。HTMLのやり取りを自分でしなくてよいので圧倒的に楽なのと、ものすごく安定しました（リンク）。
あと、データセーブ時にスマホ側からM5StickCに日時データを送信し、それに基づいてデータファイル内に日時を入れるようにしました。Java Scriptで日時を取得してM5StickC側にフォームで送信、M5StickC側ではそれをデータファイル内に入れてスマホ側に送信、スマホ側でデータファイルを保存という動きをします（リンク、リンク、リンク）。スマホの位置データも取得してデータファイルに入れたかったんですが、Java Scriptで位置データを取得するgeolocation機能はhttpsでしか使えないのと、M5StickCにhttpsを実装するのが難しそうだったので諦めました。良い方法を知ってる方がいたら教えて下さい。
上記ページなどを参考にして、スマホでM5StickCにWiFiアクセスし、WebページからLIDAR制御、測定値表示、データの保存ができるようにしました。
最後につけた制御プログラムでは、

SSID:TENOHIRA_LIDAR2
PASSWORD:なし
WebサーバーのIPアドレス：192.168.11.1

なので、スマホのWiFiでSSIDTENOHIRA_LIDAR2に接続し（「インターネット接続がありません」とか言われますが無視）、ブラウザーでhttp://192.168.11.1/に接続すればLIDARの制御画面になります。

制御画面は以下のような感じです。
スマホのコントロール画面
コマンドボタンは、

Scan0：角度を変えずに連続測定
Scan1：0～180°まで15°ステップでスキャン
Scan2：0～180°まで3°ステップでスキャン
Scan3：0～180°まで1°ステップでスキャン
LIDAR STOP：測定停止
Save　Data：測定結果をスマホに保存
です。

本体での制御

前述の回路図から分かるように本体には電源スイッチをつけてます。電源スイッチをOFFにするとM5StickCへの電源供給が絶たれるんですが、こいつ、内部バッテリーを持ってるのでバッテリーが切れるまで動き続けます。なのでM5StickCは別途電源をOFFにしなくちゃという面倒臭いことになります。これを避けるために、USB電源の電圧をモニターして、電圧が下がると（電源スイッチがOFFになると）、自分で電源を切るような制御を入れました。電源スイッチをONにしたときは勝手に立ち上がってくれます。
また、M5StickCのMボタンを押すと角度固定で連続測定を開始します（もう一回押すと停止）。スマホ画面でのScan0と同じ動作です。M5StickCの画面には取得データが表示されます。前述したように、ANGはレーザーの水平面からの角度、DSTは目標までの水平距離、HETは目標の高さです。

7. フィールドテスト

にしきひみつ基地ミュージアムでフィールドテストを行いました。

Lidar1では完敗した土壁の地下壕

断面形状がばっちり取れてます！これでゲジゲジ様も怖くない！！

8. 制御プログラム

制御プログラムです。壮大なものになってますがあまり詳細な説明は面倒くさいので、適当に読み解いてください。仕様は、

レーザー距離計モジュールはUART接続（G36、G0）
サーボはGPIOでPWM制御（G32）
M5StickCをWiFiアクセスポイントにしてスマホから接続
スマホからWebページにアクセスしてLIDARを制御、データをダウンロード
M5StickCのLCD画面に測定値を表示

プログラム構成

servoMove(float servo_angle)：サーボを指定角度に動かす（補正係数付き）
lidarMove(float angle_destination)：サーボを指定角度までゆっくり動かす
angleSearch(float search_angle)：指定角度のデータ番号を検索する
controlPage()：コントールページをスマホ側に送信する
dataTransfer()：測定データファイルをスマホ側に送信する
handleAccess()：スマホからアクセスがあるとここが実行される
handleNotFound()：スマホからの送信内容にエラーがあるとここが実行される
LIDAR(void *pvParameters)：LIDAR測定　マルチタスクで常時稼働しており、Main側でlidar_stateをLIDAR_MEASURINGに変えるとスキャンを開始します
void setup()：初期化
void loop()：メインルーチン

/////////////////////////////////////////////////////////////////////////
//  LIDAR CONTROL Program ver6      2023/8/15  Inu-Kitsune             //
//     M5Stick-C                                                       //
//     Lidar TW10S-UART                                                //
//     Servo SG92R                                                     //
/////////////////////////////////////////////////////////////////////////

#include <Arduino.h>
#include <M5StickC.h>
#include <math.h>
#include <WiFi.h>
#include <WiFiAP.h>
#include <WebServer.h>

// Glabal variables and macros
  // M5stickC 
  #define M5StickC_LED 10
  #define M5StickC_BtnA 37
  TFT_eSprite sprite(&M5.Lcd);

  // Laser distance mater
  #define PIN_LIDAR_Rx 36
  #define PIN_LIDAR_Tx 0 
  #define PIN_LIDAR_ACTIVE 26
  #define LIDAR_TIMEOUT 2000    // レーザー距離計の応答待ちタイムアウト時間
  #define LIDAR_WAIT 300        // サーボを動かしてから測定までの待ち時間（msec）
  #define DISTANCE_OFFSET 4     // レーザー距離計のオフセット長（cm）；レンズ～回転中心までの長さ
  HardwareSerial UART_LIDAR(1);

  // Servo
  #define CH_SERVO 0
  #define PIN_SERVO_GPIO 32
  #define SERVO_FREQ 50
  #define SERVO_RESO 14         // PWM制御のビット数
  #define ANGLE_OFFSET 0        // サーボのオフセット角度
  // 角度補正係数
  float ANGLE_A1 = 1.63E-05;
  float ANGLE_A2 = -4.49E-03;
  float ANGLE_A3 = 2.83E-01;
  float ANGLE_A4 = -2.32E-01;

  int SERVO_MIN = 0.45/(1000.0 / SERVO_FREQ) * pow(2, SERVO_RESO);  // サーボPWM信号の最短長さ
  int SERVO_MAX = 2.55/(1000.0 / SERVO_FREQ) * pow(2, SERVO_RESO);  // サーボPWM信号の最長長さ
  int SERVO_000 = 446 * pow(2, SERVO_RESO) / pow(2, 14);            // 0°の時のPWM信号の長さ
  int SERVO_180 = 1900 * pow(2, SERVO_RESO) / pow(2, 14);           // 180°の時のPWM信号の長さ
  float angle_stop = 180;     // スキャン時の最終角度
  float angle_step = 1.0;     // スキャンのステップ
  float angle_home = 0;       // サーボのホーム位置角度
  float angle = angle_home;   // サーボの現在角度

  // Glabal variables
  #define MAX_DATA 500        // 最大測定点
  #define LIDAR_WAITING 0     // LIDARの状態定数（0=停止状態）
  #define LIDAR_READY 1       // LIDARの状態定数（1=待機状態）
  #define LIDAR_MEASURING 2   // LIDARの状態定数（2=測定中）
  #define LIDAR_FINISH 3      // LIDARの状態定数（3=測定終了）
  #define LIDAR_CONT 1        // LIDARの測定モード（1=固定角度）
  #define LIDAR_SCAN 2        // LIDARの測定モード（2=スキャン）
  volatile int lidar_state = LIDAR_WAITING;   // LIDARの状態
  volatile int lidar_mode = LIDAR_CONT;       // LIDARの測定モード
  volatile int LIDAR_ndata = 0;               // LIDARの測定データ数
  volatile float distance = 0;                // 距離測定結果
  volatile float LIDAR_angle[MAX_DATA];       // 測定結果（角度）
  volatile float LIDAR_distance[MAX_DATA];    // 測定距離（角度）
  int autorequest = false;    // TrueならWebページが自動読み込みモードになる

  // LIDAR COMMANDS
  uint8_t lidar_ready[]        = {0x01, 0x03, 0x02, 0x00, 0x00, 0xB8, 0x44};             // Ready for measurement
  uint8_t lidar_single[]       = {0x01, 0x03, 0x00, 0x0F, 0x00, 0x02, 0xF4, 0x08};       // Sigle measurment
  uint8_t lidar_countinuous[]  = {0x01, 0x03, 0x00, 0x01, 0x00, 0x02, 0x95, 0xCB};       // Contiunuous measurment
  uint8_t lidar_stop[]         = {0x01, 0x03, 0x00, 0x0A, 0x00, 0x02, 0xE4, 0x09};       // Stop Contunuous measurment
  uint8_t lidar_laser[]        = {0x01, 0x10, 0x00, 0x03, 0x00, 0x01, 0x02, 0x00, 0x01, 0x67, 0xA3}; // Laser ON/OFF
  uint8_t lidar_9600bps[]      = {0x01, 0x10, 0x00, 0x00, 0x00, 0x02, 0x04, 0x00, 0x00, 0x25, 0x80, 0xE8, 0x9F}; // Change Serial to 9600bps
  uint8_t lidar_38400bps[]     = {0x01, 0x10, 0x00, 0x00, 0x00, 0x02, 0x04, 0x00, 0x00, 0x96, 0x00, 0x9C, 0x0F}; // Change Serial to 38400bps
  uint8_t lidar_115200bps[]    = {0x01, 0x10, 0x00, 0x00, 0x00, 0x02, 0x04, 0x00, 0x01, 0xC2, 0x00, 0xF3, 0x0F}; // Change Serial to 115200bps
  uint8_t lidar_header[]       = {0x01, 0x03, 0x04};   // Header of measurement result

  // WiFi server
  WebServer server(80);
  const char* APssid = "TENOHIRA_LIDAR2";       // AP SSID
  const char* APpassword = "";                  // AP password
  IPAddress ipadd(192,168,11,1);                // Fixed IP Address
  IPAddress subnet(255,255,255,0);              // Subnet mask

/////////////////////////////////////////////////////////////////////////
// Servo control
/////////////////////////////////////////////////////////////////////////
int servoMove(float servo_angle)       // サーボを指定角度に動かす（補正係数付き）
{
  // mapping angle to servo
  int servo_value;
  int result = true;
  float comp_angle;

  comp_angle = servo_angle+ANGLE_A1*pow(servo_angle,3)+ANGLE_A2*pow(servo_angle,2)+ANGLE_A3*servo_angle +ANGLE_A4;
  servo_value = map(int((comp_angle - ANGLE_OFFSET) * 10), 0, 1800, SERVO_000 * 10, SERVO_180 * 10) / 10;
  if(servo_value < SERVO_MIN)
  {
    servo_value = SERVO_MIN;
    result = false;
  }  
  if (servo_value > SERVO_MAX)
  {
    servo_value = SERVO_MAX;
    result = false;
  }
  ledcWrite(CH_SERVO, servo_value);
  return result;
}

void lidarMove(float angle_destination)    // サーボを指定角度までゆっくり動かす
{
  float agnle_delta;

  if(angle >= angle_destination)
  {
    agnle_delta = -1;
  }
  else
  {
    agnle_delta = 1;
  }
  for (; angle != angle_destination ; angle = angle + agnle_delta)
  {
    servoMove(angle);
    delay(20);
  }
  servoMove(angle);
}

int angleSearch(float search_angle)        // 指定角度のデータ番号を検索する
{
  int count1 = 0;
  while(LIDAR_angle[count1] != search_angle)
  {
    count1++;
    if(count1 >= MAX_DATA)
    {
      count1 = MAX_DATA - 1;
      break;
    }
  }
  return count1;
}

/////////////////////////////////////////////////////////////////////////
// HTML
/////////////////////////////////////////////////////////////////////////
void controlPage()               // コントールページをスマホ側に送信する
{
  int count1;
  String html;

  Serial.println("Send control page to client");

  html = "<!DOCTYPE html>";
  html += "<html>";
  html += "<head>";
  html += "<meta charset=\"utf-8\">";
  // 自動リフレッシュ設定
  if(autorequest == true)
  {
    html += "<meta http-equiv=\"refresh\" content=\"2; URL=/\">";
  }
  html += "<title>LIDAR2</title>";
  // データダウンロード用JAVA Script
  html += "<script>";
  html += " function saveData(){";
  // 日時情報取得
  html += "  var today = new Date();";
  html += "  var fday = today.getFullYear() + (today.getMonth()+1).toString().padStart(2, '0') + today.getDate().toString().padStart(2, '0');";
  html += "  var ftime =  today.getHours().toString().padStart(2, '0') + today.getMinutes().toString().padStart(2, '0') + today.getSeconds().toString().padStart(2, '0');";
  html += "  var ftoday = fday +'_'+ ftime;";
  // リンク作成
  html += "  var element = document.createElement('a');";
  html += "  document.body.appendChild(element);";
  html += "  element.download = 'LidarData_' + ftoday + '.txt';";
  html += "  element.href = '/Lidardata?datetime=' + ftoday;";
  html += "  element.click();}";
  html += "</script>";
  html += "</head>";
  html += "<body onload=\"draw();\" style=\"font-size:50px; color:black; text-align:center;\">";
  html += "<div style=\"font-size:120%; color:red;\">LIDAR2 Control</div><br>";
  // 測定データ表示
  html += "<table align=\"center\" border=\"1\"><tr>";
  html += "<td>Angle</td>";
  html += "<td>";
  html += String(int(angle));
  html += "deg</td>";
  html += "<td>Distance</td>";
  html += "<td>";
  html += String(distance, 1);
  html += "cm</td></tr>";
  html += "<tr><td></td><td>0deg</td><td></td><td>";
  html += String(LIDAR_distance[angleSearch(0)], 1);
  html += "cm</td></tr>";
  html += "<tr><td></td><td>90deg</td><td></td><td>";
  html += String(LIDAR_distance[angleSearch(90)], 1);
  html += "cm</td></tr>";
  html += "<tr><td></td><td>180deg</td><td></td><td>";
  html += String(LIDAR_distance[angleSearch(180)], 1);
  html += "cm</td></tr></table><br>";
  // コマンドボタン
  html += "<table align=\"center\" border=\"3\"><tr>";
  html += "<td><form><button style=\"font-size:100%;\" type=\"submit\" name=\"scan\" value=\"scan0\">Scan0(continuous)</button></form></td>";
  html += "<td><form><button style=\"font-size:100%;\" type=\"submit\" name=\"scan\" value=\"scan1\">Scan1(15deg step)</button></form></td></tr>";
  html += "<tr><td><form><button style=\"font-size:100%;\" type=\"submit\" name=\"scan\" value=\"scan2\">Scan2(3 deg step)</button></form></td>";
  html += "<td><form><button style=\"font-size:100%;\" type=\"submit\" name=\"scan\" value=\"scan3\">Scan3(1 deg step)</button></form></td></tr>";
  html += "<tr><td><form> <button style=\"font-size:100%;\" type=\"submit\" name=\"command\" value=\"stop\">LIDAR STOP</button></form></td>";
  html += "<td><input type=\"button\" style=\"font-size:100%;\" value=\"Save Data\" onclick=\"saveData()\">";
  html += "</tr></table>";
  // サーボの角度設定
  html += "<form>angle<input style=\"font-size:100%;\" type=\"number\" min=\"0\" max=\"180\" name=\"angle\" value=\"";
  html += String(int(angle));
  html += "\">";
  html += "<input style=\"font-size:100%;\" type=\"submit\" value=\"Set\"> </form><br>";
  html += "<div style=\"color:blue;\">Scan result (5m sq.)</div>";
  // 測定結果表示
  html += "<canvas id = \"chart\" width = \"1000\" height = \"520\"></canvas>";
  html += "<script type = \"text/javascript\">";
  html += "function draw()";
  html += "{";
  html += "var canvas = document.getElementById('chart');";
  html += "if (canvas.getContext) {";
  html += "var ctx = canvas.getContext('2d');";
  html += "ctx.strokeRect(20, 0, 960, 520);";
  html += "ctx.strokeRect(20, 0, 480, 520);";
  html += "ctx.strokeRect(20, 0, 960, 500);";
  for (count1 = 0; count1 < LIDAR_ndata; count1++)
  {
    html += "ctx.fillRect(";
    html += String(int(500 + LIDAR_distance[count1] * cos(LIDAR_angle[count1] * M_PI / 180)));
    html += ",";
    html += String(int(500 - LIDAR_distance[count1] * sin(LIDAR_angle[count1] * M_PI / 180)));
    html += ", 5, 5);";
  }
  html += "} }";
  html += "</script>";
  html += "</body>";
  html += "</html>";

  server.send(200, "text/html", html);
}

void dataTransfer()             // 測定データファイルをスマホ側に送信する
{
  int count1;
  String datafile;
  String datetime;
  String current_date;
  String current_time;

  // スマホから送られてきた日付データを取得
  datetime = server.arg("datetime");
  current_date = datetime.substring(0, 4) + "/" + datetime.substring(4, 6) + "/" + datetime.substring(6, 8);
  current_time = datetime.substring(9, 11) + ":" + datetime.substring(11, 13) + ":" + datetime.substring(13, 15);

  Serial.println("Send data to client");
  Serial.println(current_date + " " + current_time);
  Serial.println(server.arg("datetime"));

  // 測定データファイルを作成
  // ヘッダー
  datafile = "LIDAR data start\n";
  datafile += "Control program v6\n";
  datafile += current_date + " " + current_time + "\n";
  datafile += "angle, distance, x, y\n";
  // 測定データ
  for (count1 = 0; count1 < LIDAR_ndata; count1++)
  {
    datafile += String(LIDAR_angle[count1]) + ",";
    datafile += String(LIDAR_distance[count1], 1) + ",";
    datafile += String(LIDAR_distance[count1] * cos(LIDAR_angle[count1] * M_PI / 180), 1) + ",";
    datafile += String(LIDAR_distance[count1] * sin(LIDAR_angle[count1] * M_PI / 180), 1) + "\n";
  }
  server.send(200, "text/plain", datafile);
}

void handleAccess()               // スマホからアクセスがあるとここが実行される
{
  float angle_target;

  // スマホからのメッセージを表示
  Serial.println("Receive GET request");
  if(server.args()>0)
  {
    Serial.println("Argument: " + server.argName(0) + "=" + server.arg(server.argName(0)));
  }

  // スキャンの場合
  if (server.hasArg("scan")) 
  {
    M5.Lcd.setTextColor(YELLOW, BLACK);
    M5.Lcd.setCursor(10, 5);
    if (server.arg("scan") == "scan0")          // Scan0（角度を変えずに連続測定）
    {
      M5.Lcd.println("START SCAN0 ");
      Serial.println();Serial.println("Start Scan0");
      angle_stop = 181;
      angle_step = 0;
      lidar_mode = LIDAR_CONT;
    }
    else
    {
      if (server.arg("scan") == "scan1")        // Scan1（0-180°まで15°ステップで測定）
      {
        M5.Lcd.println("START SCAN1 ");
        Serial.println();Serial.println("Start Scan1");
        angle_stop = 180;    // スキャン最終角度
        angle_step = 15;     // 角度ステップ
      } 
      if (server.arg("scan") == "scan2")        // Scan2（0-180°まで3°ステップで測定）
      {
        M5.Lcd.println("START SCAN2 ");
        Serial.println();Serial.println("Start Scan2");
        angle_stop = 180;
        angle_step = 3;
      } 
      if (server.arg("scan") == "scan3")        // Scan2（0-180°まで1°ステップで測定）
      {
        M5.Lcd.println("START SCAN3 ");
        Serial.println();Serial.println("Start Scan3");
        angle_stop = 180;
        angle_step = 1;
      } 
      lidar_mode = LIDAR_SCAN;
      lidarMove(angle_home);
    }
    lidar_state = LIDAR_MEASURING;
    autorequest = true;
  }

  // 停止コマンドの場合
  if (server.hasArg("command")) 
  {
    if (server.arg("command") == "stop")
    {
      lidar_state = LIDAR_FINISH;
    } 
  } 

  // 角度設定コマンドの場合
  if (server.hasArg("angle"))
  {
    angle_target = server.arg("angle").toInt();
    lidarMove(angle_target);
  } 
  controlPage();
}

void handleNotFound()               // スマホからの送信内容にエラーがあるとここが実行される
 {
  server.send(404, "text/plain", "Not Found");
 }

/////////////////////////////////////////////////////////////////////////
// LIDAR measurement
//    マルチタスクで常時稼働しています
//    Main側でlidar_stateをLIDAR_MEASURINGに変えるとスキャン開始
/////////////////////////////////////////////////////////////////////////
void LIDAR(void *pvParameters)
{
  int count1;
  int datacount = 0;
  int timeout = true;
  int lidar_active = false;
  char buf;
  char data[20];
  float ax, ay, az;
  float pitch, height, hdistance;

  while(1)
  {
    datacount = 0;
    while(lidar_state == LIDAR_MEASURING)     // lidar_stateがLIDAR_MEASURINGになるとスキャン開始
    {
      if(lidar_active == false)               // 距離センサーが止まってたら連続測定コマンドを送る
      {
        UART_LIDAR.write(lidar_countinuous, sizeof(lidar_countinuous));
        digitalWrite(M5StickC_LED, LOW);
        lidar_active = true;
      }
      servoMove(angle);                       // サーボを動かす
      delay(LIDAR_WAIT);                      // LIDAR_WAIT(msec)待機
      while(UART_LIDAR.available() > 0)       // 距離センサーからの受信バッファを空にする
      {
        buf = UART_LIDAR.read();
      }
      count1 = 0;
      while(UART_LIDAR.available() < 9)       // 距離センサーからの測定データが来るまで待つ
      {
        count1++;
        delay(5);
        if(count1 > LIDAR_TIMEOUT/5)          // タイムアウトになったらエラー
        {
          Serial.println("TIMEOUT");
          distance = -1;
          timeout = false;
          break;
        }
      }
      if(timeout == true)                     // 測定データ処理開始
      {
        if(UART_LIDAR.find(lidar_header, 3) == true)
        {
          for (count1 = 0; count1 < 6; count1++)
          {
            data[count1] = UART_LIDAR.read();
          }
          distance = float((data[0]*256*256*256 + data[1]*256*256 + data[2]*256 + data[3]))/10;  // 測定データをcm単位に変換
          distance = distance + DISTANCE_OFFSET;
          if(distance < 0 || distance > 4000)
          {
            distance = -2;    // 測定値エラー
          }
        }
        else
        {
            distance = -3;    // 測定値エラー
        }
      }
      if(lidar_mode == LIDAR_CONT)      // Scan0の時の処理
      {
        M5.MPU6886.getAccelData(&ax, &ay, &az);   // 慣性センサーの測定値取得
        pitch = atan2(ay,az);                     // 傾きに変換
        hdistance = distance * cos(pitch);        // 水平距離を計算
        height = distance * sin(pitch);           // 垂直距離（高さ）を計算
        // 測定値をLCDに表示
        Serial.printf("PITCH:%f  H-DISTANCE:%f  HEIGHT;:%f\n", pitch * 180 / M_PI, hdistance, height);
        M5.Lcd.setTextColor(WHITE, BLACK);
        M5.Lcd.setCursor(0, 25);
        M5.Lcd.println("            ");
        M5.Lcd.println("            ");
        M5.Lcd.println("            ");
        M5.Lcd.setCursor(0, 25);
        M5.Lcd.println(" ANG:" + String(pitch * 180 / M_PI,0) + "deg");
        M5.Lcd.println(" DST:" + String(hdistance,0) + "cm");
        M5.Lcd.println(" HET:" + String(height,0) + "cm");
      }
      if(lidar_mode == LIDAR_SCAN)      // Scan1～3の時の処理
      {
        // 測定値をLCDに表示
        Serial.print("ANGLE(deg)/DISTANCE(cm)= ");
        Serial.print(angle,1); Serial.print("/");Serial.println(distance,1);
        M5.Lcd.setTextColor(WHITE, BLACK);
        M5.Lcd.setCursor(0, 25);
        M5.Lcd.println("            ");
        M5.Lcd.println("            ");
        M5.Lcd.println("            ");
        M5.Lcd.setCursor(0, 25);
        M5.Lcd.println(" " + String(angle,0) + "deg");
        M5.Lcd.println(" " + String(distance,0) + "cm");
        // 測定値をデータ配列に格納
        LIDAR_angle[datacount] = angle;
        LIDAR_distance[datacount] = distance;
      }
      if(angle_step != 0)
      {
        datacount++;
      }
      LIDAR_ndata = datacount;
      angle = angle + angle_step;
      timeout = true;

      if(angle > angle_stop)        // 角度がスキャン終了角度になったら測定終了（lidar_stateをLIDAR_FINISHに）
      {
        lidar_state = LIDAR_FINISH;
      }
    }
    if(lidar_active == true)        // 測定終了時に距離センサーに停止コマンドを送信
    {
      delay(500);
      UART_LIDAR.write(lidar_stop, sizeof(lidar_stop));
      digitalWrite(M5StickC_LED, HIGH);
      lidar_active = false;
      if(lidar_mode == LIDAR_CONT)
      {
        lidarMove(angle);
      }
      else
      {
        lidarMove(angle_home);      // サーボをホーム位置に戻す
        M5.Lcd.setTextColor(YELLOW, BLACK);
        M5.Lcd.setCursor(10, 5);
        M5.Lcd.println("LIDAR READY ");
        Serial.println();Serial.println("STOP MEASUREMENT");
        lidar_state = LIDAR_READY;
      }
      autorequest = false;
    }
    delay(100);
  }
}

/////////////////////////////////////////////////////////////////////////
// Main programs
/////////////////////////////////////////////////////////////////////////
void setup()
{
  char buf;

  M5.begin();
  Serial.begin(115200);

  pinMode(M5StickC_LED, OUTPUT);
  digitalWrite(M5StickC_LED, HIGH);
  pinMode(M5StickC_BtnA, INPUT_PULLUP);

  // DISPLAY
  M5.Lcd.setRotation(3);
  M5.Lcd.setTextSize(2);
  M5.Lcd.setTextColor(RED, BLACK);
  M5.Lcd.setCursor(0, 5);
  M5.Lcd.println("LIDAR2       ");
  M5.Lcd.println("Initializing ");
  Serial.println("Initializing");

  // MOTION SENSOR
  M5.MPU6886.Init();
  M5.Mpu6886.SetAccelFsr(M5.Mpu6886.AFS_2G);

  // WiFi
  Serial.println("Starting WiFi AP");
  WiFi.softAP(APssid, APpassword);
  delay(200);
  WiFi.softAPConfig(ipadd, ipadd, subnet);
  IPAddress esp32IP = WiFi.softAPIP(); 
  Serial.println("");
  Serial.println("WiFi AP configured");
  Serial.print("AP MAC: ");Serial.println(WiFi.softAPmacAddress());
  Serial.print("IP address: "); Serial.println(esp32IP);
  Serial.println("");
  M5.Lcd.println(esp32IP);
  delay(500);

  // Web server
  server.on("/", handleAccess);
  server.on("/Lidardata", dataTransfer);
  server.onNotFound(handleNotFound);
  server.begin();
  delay(500);

  // Servo 
  pinMode(PIN_SERVO_GPIO, OUTPUT);
  ledcSetup(CH_SERVO, SERVO_FREQ, SERVO_RESO);
  ledcAttachPin(PIN_SERVO_GPIO, CH_SERVO);
  servoMove(angle_home);

  // レーザー距離センサー
  pinMode(PIN_LIDAR_ACTIVE, OUTPUT);
  digitalWrite(PIN_LIDAR_ACTIVE, LOW);
  pinMode(PIN_LIDAR_Rx, INPUT);
  pinMode(PIN_LIDAR_Tx, OUTPUT);
  UART_LIDAR.begin(9600, SERIAL_8N1, PIN_LIDAR_Rx, PIN_LIDAR_Tx);     // UARTをセットアップ
  delay(200);
  digitalWrite(PIN_LIDAR_ACTIVE, HIGH);    // レーザー距離センサーをアクティブに
  while(UART_LIDAR.available() < 8)
  {
    delay(10);
  }
  M5.Lcd.fillScreen(BLACK);
  if(UART_LIDAR.find(lidar_ready, sizeof(lidar_ready)) == true)     // レーザー距離センサーが起動OKを返したら
  {
    while(UART_LIDAR.available() != 0)
    {
      buf = UART_LIDAR.read();
    }
    M5.Lcd.setTextColor(YELLOW, BLACK);
    M5.Lcd.setCursor(10, 5);
    M5.Lcd.println("LIDAR READY ");
    Serial.println("LIDAR READY");
    lidar_state = LIDAR_READY;
    xTaskCreateUniversal(LIDAR, "LIDAR", 8192, NULL, 5, NULL, APP_CPU_NUM);   // LIDAR測定タスクを開始
  }
  else
  {
    M5.Lcd.setTextColor(YELLOW, BLACK);
    M5.Lcd.setCursor(10, 5);
    M5.Lcd.println("LIDAR FAIL  ");
    Serial.println("LIDAR FAIL  ");
    lidar_state = LIDAR_WAITING;
  }
}

void loop()
{
  int laser_onoff = false;
  
  while(1)
  {
    // スマホからの接続を待つ
    server.handleClient();

    if(lidar_state == LIDAR_FINISH)   // スキャンが終了した待機状態にする
    {
      lidar_state = LIDAR_READY;
    }

    // Mボタンが押された時の処理
    M5.update();
    if(M5.BtnA.wasReleased() == true) 
    {
      if(laser_onoff == false)        // Mボタンが押されると角度固定測定（Scan0）を開始 
      {
        laser_onoff = true;
        M5.Lcd.setTextColor(YELLOW, BLACK);
        M5.Lcd.setCursor(10, 5);
        M5.Lcd.println("START SCAN0 ");
        Serial.println();Serial.println("Start Scan0");
        angle_stop = 181;
        angle_step = 0;
        lidar_state = LIDAR_MEASURING;
        lidar_mode = LIDAR_CONT;
      }      
      else                            // Mボタンがもう一度押されると待機状態に
      {
        laser_onoff = false;
        M5.Lcd.setTextColor(YELLOW, BLACK);
        M5.Lcd.setCursor(10, 5);
        M5.Lcd.println("LIDAR READY ");
        Serial.println();Serial.println("LIDAR READY");
        lidar_state = LIDAR_FINISH;
      }
    }

    // USBの電圧が低下した場合にM5StickCの電源を落とす 
    if(M5.Axp.GetVBusVoltage() < 3)
    {
      M5.Lcd.setTextColor(RED, BLACK);
      M5.Lcd.setCursor(0, 5);
      M5.Lcd.println("LIDAR2      ");
      M5.Lcd.println("SHUTDOWN    ");
      lidarMove(angle_home);
      M5.Lcd.println("GOOD BYE    ");
      M5.Lcd.println("            ");
      delay(3000);
    	M5.Axp.PowerOff();
    }
    delay(10);
  }
}

地下壕・建物の断面が測定できるポーダブルサイズのLIDAR「手のひらLIDAR ver2」の製作

目次

記載されてる技術

1.初めに

2. 全体概要