地下壕・建物の断面が測定できるポーダブルサイズのLIDAR「手のひらのLIDAR ver1」の製作
地下壕や部屋などの断面形状を測定する携帯型LIDAR（手のひらLIDAR）について書いてゆきます
 |
 |
|---|
2023/8/16 レーザー距離計を使ってレンジを広げた手のひらLIDAR ver2を作りました
目次
- はじめに
- 全体概要
- 使った部品について
- LIDARの制御
- コンパス
- スマホでの制御
- フィールドテスト
- 制御プログラム
記載されてる技術
- ロボット掃除機用のLIDARユニットの使い方（怪しい中国製部品の使いこなし）
- 磁気センサー（コンパス）の使い方
- ESP32（M5STickC）での制御
- HTTPを使ってスマホからESP32（M5STickC）をコントロール
1.初めに
地下壕などを探検してると、穴の大きさや断面形状が知りたくなることないですか？いや、知りたくなるでしょ？断面形状からいつ・誰が掘ったのかがわかるらしいし。そんな時、距離や寸法を測る機器があるととても便利。しかも自動でスキャンして断面形状を出してくれれば、もう探検もはかどりまくりです。副産物としてあなたのお部屋のサイズも測れます！これはもう作るしかないでしょう。
で、その距離を測る方法なんですが、いろいろあります。
- 超音波距離計：超音波を出して、その反射して帰ってくる時間で距離を測る
→ ビームが結構広がるので、位置精度がそんなに高くない＆数メートルが限界 - 赤外線TOF距離計：LEDで赤外線を出して、その反射して帰ってくる時間で距離を測る
→ いい感じで測れるんですが、やっぱり数メートルが限界 - レーザー距離計（TOFタイプ）：レーザーを出して、その反射して帰ってくる時間で距離を測る
→ 数10メートルまで測れますが、測定時間がかかる（数点/秒） - レーザー距離計（三角測量タイプ）：レーザーの反射して帰ってくる角度で距離を測る
→ 超高速で距離測定が可能
今回は、中国製のロボット掃除機に使われているLIDARユニット（レーザー距離計 三角測量タイプ）を買ってきて、それを地下壕の断面形状測定用のLIDARに仕上げました。ちなみに「LIDAR」はLight Detection And Rangingの略で、直訳すると光で距離を測るだけですが、通常は距離を測りながらレーザーをスキャンして、2Dか3Dのマッピングができるものを言います。
ロボット掃除機のLIDARユニットは、三角測量で距離を測る測定部と、360°スキャンする回転部が一体化されており、買ってきて使いこなせばもうそれでLIDAR出来上がりというお手軽さと、1秒もかからずに360°スキャンできる高速性に惹かれちゃいました。
 |
 |
|---|
で、結果ですが、フィールドテストのところで書きますが、イマイチでした。反射率の高いコンクリート壁の穴はきれいに断面形状が出ますが、土の壁は反射が弱く、数10cmぐらいの距離までしか測れません・・・。まあ、元がロボット掃除機なんで、ゲジゲジ付きの土の壁を測ることは想定してないよな・・・
→ 2023/8/16 レーザー距離計を使ってレンジを広げた手のひらLIDAR ver2を作りました
2. 全体概要
全体は下のような感じでまとめました。
フタを開けて測定可能状態にしたとき
フタを閉めたとき
ホームセンターで買ってきたプラスチックケースに、
- ロボット掃除機用のLIDAR
- 磁気センサー（コンパス）
- 制御用コンピューター（M5StickC）
- LIDARユニットON/OFF用のフォトモスリレー
- モバイルバッテリー
を詰め込みました。フタを開けて90°に固定すると、垂直方向に距離スキャンができるようになります。
そうそう、このフタを90°に固定するための金具。宝石箱とかオルゴールによく使ってるあの固定金具。あれの名前何？？？ 探したよ。結局、閉り止めもしくはステーというひねりも何もない名前らしい・・・ イマイチ納得できない。
回路構成はこんな感じです。
3. 使った部品について
ロボット掃除機用のLIDARユニット
これが心臓部です。AliExpressで、「LIDAR 掃除機」で検索すると、似たようなのがたくさん引っ掛かります。千円ぐらいから1万円を超えるものまで・・・。まあ高いのはそれなりに性能がいいんでしょうが、使えなかった時のリスクがな～。ということで、これを選びました。 なんでこれを選んだかというと、
- 端子の説明がある
- プログラム例が示されてる https://github.com/stdlogicvector/MB_1R2T_LIDAR_Demo
これがないと、使いこなすのがかなり面倒（っていうか不可能）。端子は+5V、GND、シリアル出力、LIDAR回転スピードの4つです。PCにUSB-Serial変換をつけて、Githubのデモプログラム（Python）を入れるとちゃんと動きました。一安心。
回転スピード端子は接続しないかLOWにすると最大回転速度（3回転/秒ぐらい）、HIGHにすると最低回転速度（1回転/秒ぐらい）になります。停止ができない・・・。しょうがないのでフォトモスリレーを入れて+5Vを直接ON/OFFするか・・・。
フォトモスリレー
フォトモスリレー（AQY211EH）を使って、制御コンピューター（M5StickC）からLIDARのON/OFFができるようにしました。だって、電源を入れると高速回転し続けるので・・・。スマホからコントロールできればかっこいいし・・・。まあ、無くても全然OKです。
磁気センサー（コンパス）
今回はコンパスもつけてLIDARがどっち向いてるか記録できるようにしました。なぜかっていうと、地下壕の中を向いて測ったのか外向いて測ったのかがわからなくなるから。それを言うと、そもそも地下壕のどこの地点の断面かがわからなくなりそうですが、それはもう仕方ないです（穴の中ではGPSも使えないので）。コンパスはI2CでつなげるCalloy GY-273 HMC5883Lを使いました。
制御用コンピューター（M5StickC）
ここは説明不要のご存じM5StickC。
M5StickCを選んだのは液晶がついてて測定値を表示させたりできるから。別に普通のESP32でも大丈夫です。マルチタスクとか使ってるのでArduinoではちょっと処理速度・メモリー不足と思います。何だったらRaspberry Pi Pico Wとかでもいいかも（LIDARのプログラム例はPythonなので）。スマホとつなぐのならWiFiかBluetoothは必須ですが。
モバイルバッテリー
そんなにパワー使わないので小さいのでも十分です。100均物でも全然OK。なんだったらM5StickCの内蔵バッテリーでも動きます（どのぐらいの時間持つかはやってませんが）。それはいいんですが、M5StickCは内蔵バッテリーのせいで逆に電源OFFがめんどくさい・・・。
閉り止め（ステー）
これが一番苦労したよ。探したよ。名前わからないし。ホームセンター行っても、100均行っても見つからなかったよ。店員さんに 「ほらあの宝石箱とかオルゴールに使う、カチッと90°に止めるやつ。ほらよくあるでしょ・・・」 って何度も聞いたよ。結局Amazonで発見。
4. LIDARユニットの制御
LIDARユニットの電源が入るとSerial Outputからは158700bpsでどんどんデータが吐き出されてきます。M5StickC側はそれをSerial Rxでどんどん受け取って、距離データに変換してゆく感じです。制御も何もありません。データはある程度の塊がパケットとして送り出されており、形式は以下のような感じです。
- パケットの開始符号：2バイト（0xAA, 0x55）
- データのヘッダー：8バイト
- データの種類 1バイト
- データ数 1バイト
- 角度の初期値 2バイト
- 角度の終了値 2バイト
- 距離データ：データ数 x 3バイト
- 反射強度データ 1バイト
- 距離データ 2バイト
→ これがデータサイズ分だけ繰り返される
なので、データ処理としては、
- 読み込んだデータから開始符号（0xAA, 0x55）を探す
- 開始符号が見つかったらその後に8バイトのヘッダーがあるので読み込み
- その後に距離データがあるので、読み込み・変換
- 次の開始符号を探す
という流れになります。処理プロトコルは上述のGithubのプログラムがありますので、それを適当にArduino形式に書き換えました。ホントにこれがなかったらどうなってたか・・・。プログラムではデータ読み込み・変換はESP32のマルチコアを使って、メインとはかかわりなく常時読み込んで処理するような制御にしました。
LIDARデータ取得部のコードは下記です。そうそう、LIDARユニットの初期角度はケースへの取り付け方で変わるので、ANGLE_OFFSETで補正しています。
LIDRAデータ取得コード
#include <Arduino.h>
#include <M5StickC.h>
#include <math.h>
#define PIN_LIDAR_Rx 36
#define PIN_LIDAR_Tx 0
#define PIN_LIDAR_ACTIVE 26
#define DATA_SYNC 0
#define DATA_HEADER 1
#define DATA_DATA 2
#define DATA_END 3
#define ANGLE_OFFSET -700
HardwareSerial UART_LIDAR(1);
volatile float LIDAR_data[360];
/////////////////////////////////////////////////////////////////////////
// LIDAR task //////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
void LIDAR(void *pvParameters)
{
UART_LIDAR.begin(158700, SERIAL_8N1, PIN_LIDAR_Rx, PIN_LIDAR_Tx);
int count1, count2;
int data_count = 0;
long time = millis();
int state = DATA_SYNC;
char header[8];
char data[256];
char sync[] = {0xAA, 0x55};
int package_type, package_size, package_start, package_stop;
int diff;
float step;
int datanum = 0;
int intensity, distance, angle;
float distancef, anglef;
while(1)
{
if(ONOFF == true)
{
time = millis(); // 一定時間応答がないと処理中止
while(UART_LIDAR.available() > 10)
{
// パケットヘッダー（0xAA, 0x55）の検出
if(state == DATA_SYNC)
{
if(UART_LIDAR.find(sync,2) == true)
{
state = DATA_HEADER;
time = millis();
}
}
// データヘッダーの読み込み
if((state == DATA_HEADER) && (UART_LIDAR.available() > 8))
{
UART_LIDAR.readBytes(header, 8);
package_type = header[0];
package_size = header[1]; // パケットのデータサイズ
package_start = (header[3] << 8) | header[2]; // パケットの角度のスタート値
package_stop = (header[5] << 8) | header[4]; // パケットの角度のエンド値
if(package_type & 0x01)
{
state = DATA_SYNC;
}
else
{
if(package_stop >= package_start)
{
diff = package_stop - package_start;
}
else
{
diff = 0xB400 - package_start + package_stop;
}
step = float(diff) / (float(package_size) - 1.0); // 角度ステップの計算
state = DATA_DATA;
datanum = 0;
}
time = millis();
}
// 距離データの読み込み
if((state == DATA_DATA) && (UART_LIDAR.available() > package_size * 3))
{
UART_LIDAR.readBytes(data, package_size * 3);
state = DATA_END;
time = millis();
}
// 距離データの変換
if(state == DATA_END)
{
for(count1 = 0; count1 < package_size; count1++)
{
intensity = data[count1 * 3 + 0];
distance = (data[count1 * 3 + 2] << 8) | data[count1 * 3 + 1];
distancef = float(distance) / 40.0; // 実距離に変換
if(distancef > 1000) distancef = 0; // 10m以上はエラー扱いにする
// 角度を計算 0xB400 = 360°
// 角度のステップは0.67° → 切りよく1°に四捨五入する！
angle = int(package_start + step * count1 + ANGLE_OFFSET) % 0xB400;
anglef = (float(angle) / 0xB400) * 360;
LIDAR_data[int(anglef)] = distancef;
}
state = DATA_SYNC;
time = millis();
}
delay(1);
if(time + 1000 < millis()) // 1secで処理が終わらなかったらエラーと判断
{
state = DATA_SYNC;
UART_LIDAR.end();
delay(100);
UART_LIDAR.begin(158700, SERIAL_8N1, PIN_LIDAR_Rx, PIN_LIDAR_Tx);
time = millis();
break;
}
}
}
delay(10);
}
}
5. コンパス
コンパスはI2C制御です。もう普通にAdafruitのライブラリーとプログラム例を元にしてます。そこまではいいんですが、出てくるデータはX、Y、Z軸方向の磁場の値。これを方位に直すのがちょっと手間がいります。今回は、
// Azimuth = atan2(X', Y') + r X' = ax(x+bx) Y' = ay(y+by)
ax = 0.451; bx = -0.934; ay = 0.756; by = 9.893; r = 1.672;
compass_x = event.magnetic.x;
compass_y = event.magnetic.y;
compass_angle = (atan2(ax * (compass_x + bx), ay * (compass_y + by)) + r)*180/M_PI;
みたいな感じで変換しました。X方向、Y方向の磁場値を比例補正した値を用いて方位角を計算し、それをrだけ回しています。で、ちゃんと北が0°、西が90°、南が180°、東が270°になるように、ax, bx, ay, by, rの値を決めます。どうやって決めたかというと・・・
- コンパスを東西南北に向けてそれぞれcompass_x、compass_y値を取る
- ax, bx, ay, by, rを適当に入れて、東西南北の場合のcompass_angleを計算
- だいたい北が0°、西が90°、南が180°、東が270°になるようにax, bx, ay, by, rを荒調整
- 最後は最適化を回して微調整
これをExcelでやりました。最適化はSolver。これをちゃんと説明すると1本文書が書けるぐらいなので、またの機会に（あるのか？？）
6. スマホでの制御
手のひらLIDARのコントロール、測定データ表示、データ保存はスマホでできるようにしました。だってPC持って地下壕に入りたくないし。M5StickCをWiFiアクセスポイントにして、スマホをWiFi接続し、HTMLでコントロールします。その辺（他もだけど）はあまり詳しくないので、もうネットにあるプログラム例を大参考にさせてもらいました。
ESP32のWiFiアクセスポイントの実装とHTMLによる制御
https://a-tomi.hatenablog.com/entry/2020/09/27/212032
HTMLでグラフを表示させる方法
https://www.genius-web.co.jp/blog/web-programming/draw-graphic-on-htmlusing-the-canvas-elements-basic-story.html
素晴らしい仕事、ありがとうございます。
上記ページを参考にして、スマホでM5StickCにWiFiアクセスし、WebページからLIDAR制御、測定値表示、データの保存ができるようにしました。最後につけた制御プログラムでは、
- SSID:TENOHIRA_LIDAR
- PASSWORD:なし
- WebサーバーのIPアドレス：192.168.11.1
なので、スマホのWiFiでSSID「TENOHIRA_LIDAR」に接続し（「インターネット接続がありません」とか言われますが無視）、ブラウザーで「http://192.168.11.1/」に接続すればLIDARの制御画面になります。
制御画面は以下のような感じです。
- Distance 0/90/180deg：0度、90度、180度方向の距離測定結果
- LIDRA ACTIVATE：LIDAR ON、回転開始
- LIDAR OFF：LIDARの停止
- GET DATA：データをスマホに保存（テキスト形式なのでExcelで読めます）
- LIDAR RESET：不具合が出た際にM5STickCをリセットできます
画面下方のグラフはLIDARの測定結果で、中央下が0点で、左右が4.3m、上が5mです。上の測定例は我が家の部屋の一部ですな。ちゃんと四角い形が出ています。データはWebページを再読み込みするとアップデートされます。
7. フィールドテスト
にしき ひみつ基地ミュージアムでフィールドテストを行いました。
まずはコンクリートの地下壕
 |
 |
|---|
 |
 |
|---|
地下壕の断面形状がばっちり取れてます！まじで研究に使えそう。
続いて土壁の場所
 |
 |
|---|
 |
 |
|---|
うーん、近いところはそこそこ測れてるんだけど、1mも離れると距離が測定できない・・・。土の壁はレーザーが乱反射されてダメみたいです（壁におられるゲジゲジ様がレーザーを吸収していると唱える識者もいるらしい）。
というわけで、ロボット掃除機のLIDRAユニットで地下壕の断面形状を測定しようとしましたが、結果はイマイチでした。反射率の高いコンクリートの壁や、部屋の中ならばっちり使えますので、用途を限定すれば有効なツールになりそうです。何より超高速なのがよい。360°スキャンができるように改良しようかな・・・いや、その前に土壁でも測定できるようにレーザー距離計TOFタイプでトライかな・・・。
8. 制御プログラム
最後に制御プログラムです。そこそこ壮大なものになってます。あまり詳細な説明は面倒くさいので、適当に読み解いてください。仕様は上にも書きましたが、
- LIDARユニットはUART接続
- コンパスはI2C接続
- M5StickCをWiFiアクセスポイントにしてスマホから接続
- スマホからWebページにアクセスしてLIDARを制御、データをダウンロード
- LIDARからのデータはマルチタスクで常時受信
→ HTMLでリクエストがあると最新データを送信 - M5StickCのLCD画面に測定値を表示
/////////////////////////////////////////////////////////////////////////
// //
// LIDAR CONTROL Program 2023/3/26 Inu-Kitsune //
// M5Stick-C //
// Lidar for vacuum cleaner //
// //
/////////////////////////////////////////////////////////////////////////
#include <Arduino.h>
#include <M5StickC.h>
#include <Wire.h>
#include <WiFi.h>
#include <WebServer.h>
#include <Adafruit_HMC5883_U.h>
#include <math.h>
#define M5StickC_LED 10
#define M5StickC_BtnA 37
#define I2C_SDA 32
#define I2C_SCL 33
#define PIN_LIDAR_Rx 36
#define PIN_LIDAR_Tx 0
#define PIN_LIDAR_ACTIVE 26
#define DATA_SYNC 0
#define DATA_HEADER 1
#define DATA_DATA 2
#define DATA_END 3
#define ANGLE_OFFSET -700
TFT_eSprite sprite(&M5.Lcd);
HardwareSerial UART_LIDAR(1);
// Glabal variables
volatile int ONOFF = false;
volatile float LIDAR_data[360];
// HMC5883 Compass
Adafruit_HMC5883_Unified compass = Adafruit_HMC5883_Unified(00011);
volatile float compass_x = 0; //Compass value X
volatile float compass_y = 0; //Compass value Y
volatile float compass_z = 0; //Compass value Z
volatile float compass_angle = 0; //Compass Angle
// HMC5883 Compass
int Compass()
{
sensors_event_t event;
compass.getEvent(&event);
compass_x = event.magnetic.x;
compass_y = event.magnetic.y;
compass_z = event.magnetic.z;
if(compass_x < 100 && compass_x > -100 && compass_y < 100 && compass_y > -100 && compass_z < 100 && compass_z > -100)
{
// Compass calibration: KISHINO original
// θ''=atan2(X', Y')+r X'=ax(x+bx) Y'=ay(y+by)
// 2023/4/8 Calib
// ax bx ay by r
// 0.451 -0.934 0.756 9.893 1 0 1.672
float ax = 0.451;
float bx = -0.934;
float ay = 0.756;
float by = 9.893;
float r = 1.672;
compass_angle = (atan2(ax * (compass_x + bx), ay * (compass_y + by)) + r)*180/M_PI;
if(compass_angle < 0) compass_angle += 360;
if(compass_angle > 360) compass_angle -= 360;
return true;
}
return false;
}
/////////////////////////////////////////////////////////////////////////
// WiFi server /////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
WiFiServer server(80);
WiFiClient client;
// WiFi
const char* APssid = "TENOHIRA_LIDAR"; // AP hostname
const char* APpassword = ""; // AP password
IPAddress ipadd(192,168,11,1); // M5StickCのIPアドレス → 適当に変えてね
IPAddress subnet(255,255,255,0);
/////////////////////////////////////////////////////////////////////////
// LIDAR task //////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////
void LIDAR(void *pvParameters)
{
UART_LIDAR.begin(158700, SERIAL_8N1, PIN_LIDAR_Rx, PIN_LIDAR_Tx);
int count1, count2;
int data_count = 0;
long time = millis();
int state = DATA_SYNC;
char header[8];
char data[256];
char sync[] = {0xAA, 0x55};
int package_type, package_size, package_start, package_stop;
int diff;
float step;
int datanum = 0;
int intensity, distance, angle;
float distancef, anglef;
while(1)
{
if(ONOFF == true)
{
time = millis(); // 一定時間応答がないと処理中止
while(UART_LIDAR.available() > 10)
{
// パケットヘッダー（0xAA, 0x55）の検出
if(state == DATA_SYNC)
{
if(UART_LIDAR.find(sync,2) == true)
{
state = DATA_HEADER;
time = millis();
}
}
// データヘッダーの読み込み
if((state == DATA_HEADER) && (UART_LIDAR.available() > 8))
{
UART_LIDAR.readBytes(header, 8);
package_type = header[0];
package_size = header[1]; // パケットのデータサイズ
package_start = (header[3] << 8) | header[2]; // パケットの角度のスタート値
package_stop = (header[5] << 8) | header[4]; // パケットの角度のエンド値
if(package_type & 0x01)
{
state = DATA_SYNC;
}
else
{
if(package_stop >= package_start)
{
diff = package_stop - package_start;
}
else
{
diff = 0xB400 - package_start + package_stop;
}
step = float(diff) / (float(package_size) - 1.0); // 角度ステップの計算
state = DATA_DATA;
datanum = 0;
}
time = millis();
}
// 距離データの読み込み
if((state == DATA_DATA) && (UART_LIDAR.available() > package_size * 3))
{
UART_LIDAR.readBytes(data, package_size * 3);
state = DATA_END;
time = millis();
}
// 距離データの変換
if(state == DATA_END)
{
for(count1 = 0; count1 < package_size; count1++)
{
intensity = data[count1 * 3 + 0];
distance = (data[count1 * 3 + 2] << 8) | data[count1 * 3 + 1];
distancef = float(distance) / 40.0; // 実距離に変換
if(distancef > 1000) distancef = 0; // 10m以上はエラー扱いにする
// 角度を計算 0xB400 = 360°
// 角度のステップは0.67° → 切りよく1°に四捨五入する！
angle = int(package_start + step * count1 + ANGLE_OFFSET) % 0xB400;
anglef = (float(angle) / 0xB400) * 360;
LIDAR_data[int(anglef)] = distancef;
}
state = DATA_SYNC;
time = millis();
}
delay(1);
if(time + 1000 < millis()) // 1secで処理が終わらなかったらエラーと判断
{
state = DATA_SYNC;
UART_LIDAR.end();
delay(100);
UART_LIDAR.begin(158700, SERIAL_8N1, PIN_LIDAR_Rx, PIN_LIDAR_Tx);
time = millis();
break;
}
}
}
delay(10);
}
}
// スマホ表示用HTML
void htmltransfer()
{
int count1;
client.print(F("<!DOCTYPE html>"));
client.print(F("<html>"));
client.print(F("<head>"));
client.print(F("<meta charset=\"utf-8\">"));
client.print(F("<title>LIDAR</title>"));
client.print(F("</head>"));
client.print(F("<body onload=\"draw();\">"));
client.print(F("<font size=15 clolor=black><br>- LIDRA Control -<br>"));
client.print(F("<br>"));
client.print(F("Distance 0deg.: "));
client.print(LIDAR_data[0], 1);
client.print(F("<br>"));
client.print(F("Distance 90deg.: "));
client.print(LIDAR_data[90], 1);
client.print(F("<br>"));
client.print(F("Distance 180deg.: "));
client.print(LIDAR_data[180], 1);
client.print(F("<br>"));
client.print(F("Azimuth: "));client.print(compass_angle, 1);
client.print(F("<br>"));
client.print(F("<ul><li>>> <a href=\"/on\"><font color=red>LIDAR ACTIVATE</font></a></li><br>"));
client.print(F("<li>>> <a href=\"/off\"><font color=green>LIDAR OFF</font></a></li><br>"));
client.print(F("<li>>> <a href=\"/get\" download><font color=blue>GET DATA</font></a></li></br>"));
client.print(F("<li>>> <a href=\"/reset\"><font color=blue>LIDAR RESET</font></a></li></ul>"));
client.print(F("</font>"));
client.print(F("<canvas id = \"sample\" width = \"1000\" height = \"520\"></ canvas>"));
client.print(F("<script type = \"text/javascript\">"));
client.print(F("function draw()"));
client.print(F("{"));
client.print(F("var canvas = document.getElementById('sample');"));
client.print(F("if (canvas.getContext) {"));
client.print(F("var ctx = canvas.getContext('2d');"));
client.print(F("ctx.strokeRect(20, 0, 860, 520);"));
client.print(F("ctx.strokeRect(20, 0, 430, 520);"));
client.print(F("ctx.strokeRect(20, 0, 860, 500);"));
for (count1 = 0; count1 < 180; count1 += 2)
{
client.print(F("ctx.fillRect("));
client.print((450 - LIDAR_data[count1] * cos(float(count1) * M_PI / 180)), 0);
client.print(F(","));
client.print((500 - LIDAR_data[count1] * sin(float(count1) * M_PI / 180)), 0);
client.print(F(", 5, 5);"));
}
client.print(F("} }"));
client.print(F("</script>"));
client.print(F("}"));
client.print(F("</body>"));
client.print(F("</html>"));
}
// スマホへの測定データ転送
void datatransfer()
{
int count1;
client.println(F("LIDAR data"));
client.print(F("Azimuth: "));client.println(compass_angle, 1);
client.println(F("angle, distance, x, y"));
for (count1 = 0; count1 < 360; count1++)
{
client.print(count1);client.print(F(","));
client.print(LIDAR_data[count1], 1);client.print(F(","));
client.print((-1 * LIDAR_data[count1] * cos(float(count1) * M_PI / 180)), 1);client.print(F(","));
client.println((LIDAR_data[count1] * sin(float(count1) * M_PI / 180)), 1);
}
client.println(F("LIDAR data end"));
}
// セットアップ
void setup()
{
int loop = 0;
int ready_Compass = 0;
Serial.println("Initializing");
M5.begin();
Serial.begin(115200);
Wire.begin(I2C_SDA,I2C_SCL);
pinMode(M5StickC_LED, OUTPUT);
digitalWrite(M5StickC_LED, HIGH);
pinMode(M5StickC_BtnA, INPUT_PULLUP);
// WiFi
Serial.printf("Starting WiFi AP");
WiFi.softAP(APssid, APpassword);
delay(100);
WiFi.softAPConfig(ipadd, ipadd, subnet);
Serial.println("");
Serial.println("WiFi AP configured");
Serial.print("AP MAC: ");Serial.println(WiFi.softAPmacAddress());
Serial.print("IP address: "); Serial.println(WiFi.softAPIP());
Serial.println("");
server.begin();
// DISPLAY
M5.Lcd.setRotation(1); // ボタンBが上になるような向き
M5.Lcd.setTextColor(YELLOW, BLACK);
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(10, 5);
M5.Lcd.println("LIDAR OFF ");
// COMPASS
while((ready_Compass == 0) && (loop < 10)) {
ready_Compass = compass.begin();
loop++;
delay(20);
}
// LIDAR
pinMode(PIN_LIDAR_ACTIVE, OUTPUT);
digitalWrite(PIN_LIDAR_ACTIVE, LOW);
xTaskCreateUniversal(LIDAR, "LIDAR", 8192, NULL, 5, NULL, APP_CPU_NUM);
Serial.println("Initialized");
}
// メインループ
void loop()
{
int count1;
int LEDONOFF = true;
String inMsg;
while(1)
{
client = server.available();
if (client)
{
Serial.println("Accessed");
inMsg = "";
while (client.connected())
{
if (!client.available())
{
break;
}
char c = client.read();
Serial.write(c);
if (c == '\n')
{
if (inMsg.length() == 0)
{
Compass();
htmltransfer();
break;
}
else
{
inMsg = "";
}
}
else if (c != '\r')
{
inMsg += c;
}
if (inMsg.endsWith("GET /on"))
{
Serial.println();Serial.println("LIDAR ACTIVE");
ONOFF = true;
}
if (inMsg.endsWith("GET /off"))
{
Serial.println();Serial.println("LIDAR AOFF");
ONOFF = false;
}
// Output LIDAR data
if (inMsg.endsWith("GET /get"))
{
Compass();
datatransfer();
break;
}
if (inMsg.endsWith("GET /reset"))
{
esp_restart();
}
}
client.stop();
Serial.println("Client Disconnected.");
}
M5.update();
if(M5.BtnA.wasReleased() == true) // Mボタンを押すとLIDARのON/OFF切り替え
{
if(ONOFF == false)
{
Serial.println();Serial.println("LIDAR ACTIVE");
ONOFF = true;
}
else
{
Serial.println();Serial.println("LIDAR OFF");
ONOFF = false;
}
}
if(ONOFF == true)
{
digitalWrite(PIN_LIDAR_ACTIVE, HIGH);
// ディスプレイ表示
M5.Lcd.setTextColor(YELLOW, BLACK);
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(10, 5);
M5.Lcd.println("LIDAR ACTIVE");
M5.Lcd.setTextColor(WHITE, BLACK);
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(0, 30);
M5.Lcd.println(" 0deg: ");
M5.Lcd.println(" 90deg: ");
M5.Lcd.println(" 180deg: ");
M5.Lcd.setCursor(0, 30);
M5.Lcd.println(" 0deg:" + String(int(LIDAR_data[0])));
M5.Lcd.println(" 90deg:" + String(int(LIDAR_data[90])));
M5.Lcd.println(" 180deg:" + String(int(LIDAR_data[180])));
if(LEDONOFF == true)
{
digitalWrite(M5StickC_LED, LOW);
LEDONOFF = false;
}
else
{
digitalWrite(M5StickC_LED, HIGH);
LEDONOFF = true;
}
}
else
{
digitalWrite(PIN_LIDAR_ACTIVE, LOW);
M5.Lcd.setTextColor(YELLOW, BLACK);
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(10, 5);
M5.Lcd.println("LIDAR OFF ");
digitalWrite(M5StickC_LED, HIGH);
}
delay(100);
}
}
Discussion