🦊
地下壕・建物の断面が測定できるポーダブルサイズのLIDAR「手のひらLIDAR ver3」の製作2 ースタンドアロン化ー
地下壕や部屋などの断面形状を測定する携帯型LIDAR（手のひらLIDAR）の第3弾をスマホ無しのスタンドアロンで使えるようにしました。本体のボタンでOLEDに表示されるメニューを選択することで全機能を使えます。測定結果は内部に保存され、WiFi接続することでスマホやPCにダウンロードできます。もちろん、これまで通りスマホ連携でも使えます！いやあ、もう、市販できるレベルじゃないか？？と思う今日この頃ですわ。
 |
 |
|---|
目次
- はじめに
- 全体概要
- 使った部品について
- Real-time Clock（RTC）モジュールの制御
- 内蔵フラッシュメモリーへのデータ保存、読み出し（SPIFFS）
- 階層メニュー制御
- 制御プログラム
記載されてる技術
- Real-time Clock（RTC）の制御
- SPIFFSを使ったESP32内蔵Flushへのデータの保存と読み出し
- スイッチと連携した階層メニュー制御（自己流！）
1.初めに
地下壕などを探検していて、その大きさや断面形状がなぜだかとても気になるあなたに、職人手作り感満載の手のひらLIDAR ver1、手のひらLIDAR ver2、手のひらLIDAR ver3を作りました。で、機構的にはこのLIDAR3が最終形だと思ってるんですが、一点だけ心残りがありました。それが、「スマホ・PCと連携する必要があり、スタンドアロンで使えない」というところです。いや、機能的に使えるようにはできるんですが、ちょっと問題があります。ここにも書いてるんですが、
- スタンアロンで使う＝測定データをLIDAR自身に保存する必要がある
- 制御に使ってるESP32はフラッシュメモリーが内蔵されてるので保存は可能
- 問題は保存した多数のデータをどうやって区別するか？
- Data1、Data2、・・ってやると、後で「このデータ、どこのデータだっけ？？」ってなる（なるよね、絶対）
- 一番良いのは測定日時で区別すること（地下壕の中では位置情報は使えまへん）
- ということはLIDAR側が現在日時を知らなきゃならない
- ESP32は電源を切ると全部忘れる
- ということは、電源を入れるたびに日時を合わせ直さないといけない
- そのためにはスマホに接続する必要がある
- だったらスタンドアロンじゃなくてスマホ連携で使えや
ああ、長かった。要はESP32がスタンドアロンで日時を知れるように、精度が良くて電源を切っても動き続ける時計（RTC；Real-time Clock）を追加すれば、問題なく保存データに測定日時を記録できる＝後でどこのデータか区別しやすくなるということですわ。そんだけのためにRTCをつけて、ついでに階層メニューを作ってフル機能の測定ができるようにしましたわ。いやもう大変でしたわ。
2. 全体概要
全体は下のような感じでまとめました。
矢印が今回追加したRTCモジュール
OLEDに表示されるメニューからコントロール
スマホやPCからも操作可能 測定データはダウンロード可能
全体のブロック図
下のように手のひらLIDAR3にI2C接続のRTCが追加されただけです。今回の変更点は主にソフトですね。
3. 使った部品について
Real-time Clock（RTC）モジュール
RTCは、まあ早い話が時計です。一度日時を合わせてしまえばESP32側の電源を切っても内蔵電池で勝手に時を刻み続けます。で、次回ESP32の電源を入れたときに現在時刻を読み出すことができます。勝手に時を刻むのでその正確さが命です。実はESP32には内蔵のRTCがあるんですが、こいつは基準発振器にキャパシターを使っており精度がよろしくない。噂だと1日に数秒狂うということです。で、外付けの高精度なのが必要なんですが、小っちゃくて安いといことでこれを採用しました。電池までついて200円以下です。使ってるICのDS3231は水晶振動子を使ってるので精度も良いらしい（まあ、本物ならばですが・・・）。電池は充電式じゃないので数年で切れると思われます。そのうち充電池と交換しよう。いや、そんな何年もLIDAR使うんか？？
その他の部品については手のひらLIDAR ver3を参照してくだされ。
4. Real Time Clock（RTC）モジュールの制御
上のブロック図に示したように、RTCモジュールはI2CでESP32と接続します。小さな部品なのでケースの片隅に転がしてあります。制御ですが、普通にライブラリーを
#include <RTClib.h>
でインクルードすれば使えます。時刻のセットは、
rtc.adjust(DateTime rtctime);
現在時刻の読み出しは、
DateTime rtctime = rtc.now();
です。でも実はここは重要じゃないっす。ホントの問題はどうやって時刻をセットするかっす。正確といっても要は普通のクォーツ時計なんでまあ月に何秒かは狂います。最初に合わせてそれっきりってわけにもいかないっしょ。
RTCの時刻をセットする
で、ネットを見ると「WiFiでネットにつないでNTPサーバーに接続して時刻合わせする方法」がもうたくさんたくさんたくさんたくさん出てきます（あと少しの「手動で合わせる」）。いや、いいんだけど、お外にはWiFiは無いっしょ！基本インターネットに繋がずに使いたい!なので今回は、スマホの時刻をもらって合わせるようにしました（自己流ですが）。
やり方ですが、
- スマホに表示されるコントロールページに「時刻セットボタン」を追加
- ボタンが押されるとJavaScriptが動いて、スマホから時刻を取得
- その時刻をESP32側にPOSTで送信
- それを受けたESP32側で時刻をセット
です。
ESP32にスマホからアクセスされたときに下記のようなページを表示させれば、スマホ側で「SET TIME」ボタンを押すとJavaScriptが動いてスマホの時刻を取得、ESP32側に
setdatetime=YYYYMMDD_HHMMSS
という形式で送信されます。
<!DOCTYPE html>
<html>
<head>
<script type = "text/javascript">
function setRTC()
{
var today = new Date();
var fday = today.getFullYear() + (today.getMonth()+1).toString().padStart(2, '0') + today.getDate().toString().padStart(2, '0');
var ftime = today.getHours().toString().padStart(2, '0') + today.getMinutes().toString().padStart(2, '0') + today.getSeconds().toString().padStart(2, '0');
var ftoday = fday +'_'+ ftime;
var form = document.createElement('form');
form.action = '/service';
form.method = 'POST';
var que = document.createElement('input');
que.value = ftoday;
que.name = 'setdatetime';
form.appendChild(que);
document.body.appendChild(form);
form.submit();
}
</script></head>
<body>
<input type="button" value="SET TIME" onclick="setRTC()">
</body></html>
で、受けたESP32側で、
void setRTC()
{
String datetime;
int current_year, current_month, current_day;
int current_hour, current_minute, current_second;
// スマホから送られてきた日付データを取得
datetime = server.arg("setdatetime");
current_year = datetime.substring(0, 4).toInt();
current_month = datetime.substring(4, 6).toInt();
current_day = datetime.substring(6, 8).toInt();
current_hour = datetime.substring(9, 11).toInt();
current_minute = datetime.substring(11, 13).toInt();
current_second = datetime.substring(13, 15).toInt();
rtc.adjust(DateTime(current_year, current_month, current_day, current_hour, current_minute, current_second));
}
としてRTCに日時をセットします。
これで、スマホの時計さえちゃんとあってれば、いつでもどこでも時刻合わせができます。たとえ地下指令室の中でも。
5. 内蔵フラッシュメモリーへのデータ保存、読み出し（SPIFFS）
LIDARの制御に使ってるESP32はフラッシュメモリーを内蔵しており、設定や測定データを記憶しておけます。今回のプログラムでは、
- LIDARの基本設定（実は今回はプログラムのバージョン記憶ぐらいにしか使ってない）
- コンパスのキャリブレーションデータ
- LIDARの距離測定データ
- LIDARのスキャン測定データ
の4種類のデータを保存しています。1、2は起動時に読み込んで初期設定に使用します。3、4は測定時に保存し、後でデータを読み出してOLEDに表示したり、スマホで表示したり、ダウンロードしたりできるようにしました。ESP32のフラッシュメモリーの容量は4MB～16MBの複数タイプがある＋プログラムに使う容量もあるのでどんだけの記憶容量が使えるかは状況次第です。まあでもLIDARのスキャンデータってせいぜい数kBなので、なんだかんだで数100個ぐらいのデータは保存できそうです。フラッシュの容量は十分あるので問題ないっすね、ということでメモリー残量を確認して「これ以上保存できません」みたいなチェックはしてません。容量オーバーしちゃったらどうなるかわかりまへん。ｷﾞｷﾞｷﾞｷﾞｷﾞ
フラッシュへの書き込みと読み出し
ESP32内蔵フラッシュメモリーを使うにはSPIFFS（SPI Flash File System）という機能を使います。使い方はネットにあふれてるのでそちらを参照です。下は例で、test.txtというファイルを書いては読み、書いては読みを繰り返します。
#include <SPIFFS.h>
void setup()
{
Serial.begin();
SPIFFS.begin();
// test.txtファイルがない場合（初回）は記憶領域をフォーマットする
if (!SPIFFS.exists("/test.txt"))
{
SPIFFS.format();
Serial.println("SPIFFS formatted");
}
}
void main()
{
File fp;
int a = 0;
// フラッシュへ書き込み
fp = SPIFFS.open("/test.txt", FILE_WRITE);
fp.println("helllo:" + String(a));
fp.close();
delay(500);
// フラッシュから読み出し
fp = SPIFFS.open("/test.txt", FILE_READ);
Serial.println(fp.readStringUntil('\n'));
fp.close();
a++;
delay(500);
}
スマホ、PCへのデータの転送
SPIFFSを使えば内蔵フラッシュメモリーへの保存・読み出しができますが、RTCと同じようにそれは大した問題じゃございません。ネット見ればたくさんたくさんたくさんたくさんたくさん出てきます。じゃなくて、それをどうやってスマホやPCへ転送するかがポイントです。ESP32をUSBメモリのようにPCにつないで内部のファイルを読むみたいなことができれば簡単なんですが、そんなステキな機能はデフォルトではついてない。そこで今回はスマホにHTMLでデータを転送するようにしました。EPS32側はスマホからのアクセスに対し、保存されてるファイル名リストが載ったWebページを返します。ユーザーはそこからファイルを選択してESP32側にリクエストすると、そのファイルがダウンロードされます。実はESP32ではフラッシュに保存されてるファイル名やファイル内容を読み出して、そのデータをHTMLに乗せてスマホに送ってるんですが、スマホ側から見ると直接ファイルを操作してるように感じるちょっとクールなインターフェースにしました。ここからファイルの削除もできます。
DOWNLOADボタンでファイルをスマホ側にダウンロードできます。この辺、ESP32というよりHTMLのテクニックですね。
で、どうやって指定したデータをスマホ側でダウンロードするかですが、これが超難航しました。手続きとしては、
- スマホ側からダウンロードしたいファイル名をESP32側に送る → これは簡単
- ESP32側ではそれを受けてデータを準備して送信する → これも簡単
- スマホ側ではそれを自動的にファイルに保存 → ！
なんですが、3が難しい。1は普通にFormでファイル名を送れば簡単、それに基づいてESP32側でファイルを読んでテキストでスマホに返す2も簡単。で、受け取ったスマホのブラウザーはそれをバカ正直に表示してしまいます。いや、表示させるんじゃなくて保存したいんだけど・・・。変なところでバカ正直です。まあ、表示されたページを手動で保存すればいいんですが、画竜点睛を欠く的に嫌っす。自動的にファイル保存する方法としては<a>タグのdownloadオプションがあるんですが、これは逆にリンク先をhref=で指定しなくちゃならない。そうするとESP32側で各ファイル名に対応したリンクを動的に準備しなくちゃならなくて、それはそれでどうすりゃいいの？状態です。
で、いろいろ調べてみた結果、UriBracesというウルトラテクノロジーでできることがわかりました。ここ参照です。
まずスマホ側で
<!DOCTYPE html>
<html>
<body>
<a href="/LIDAR_〇〇〇" download><button>DOWNLOAD</button></a>
</body>
</html>
というページを表示させます（〇〇〇のところには欲しいファイル名が入る）。ページでDOWNLOADボタンが押されると、/LIDAR_〇〇〇というリンク先をリクエストして、返ってきたデータを自動保存します。
これを受ける側の関数はこんな感じです。
#include <uri/UriBraces.h>
・・・・
server.on(UriBraces("/LIDAR_{}"), spiffsTransfer);
・・・・
void spiffsTransfer()
{
String datafile;
String fname = server.pathArg(0);
File fp = SPIFFS.open("/" + fname, FILE_READ);
while(fp.available())
{
datafile += fp.readStringUntil('\n') + "\n";
}
Serial.println(datafile);
server.send(200, "text/plain", datafile);
}
/LIDAR_〇〇〇というリンクに飛ぶようにリクエストされると、server.on(UriBraces("/LIDAR_{}"), spiffsTransfer);という設定に基づき関数spiffsTransfer()が呼び出されます。関数内でfname = server.pathArg(0)とすれば〇〇〇（ファイル名）を取り出せます。で、〇〇〇のデータをスマホ側に送信する。スマホ側ではそのデータがあたかもリクエストした/LIDAR_〇〇〇というリンク先だと感じてダウンロード・保存します。JOHN M. WARGOさん天才や！！（ひょっとしてHTML界では常識？？）
6. 階層メニュー制御
さあ、今回の目玉の階層メニュー制御です。と言っても自己流なので効率の点では？？？かもしれません。まあ、でも、動けばOKってことで。
メニューの階層構造
OLEDにメニューを表示し、↑↓キーで選択し、GOキーで実行という使い方です。後で気づいたんですが、操作スイッチがディスプレイの左側にあるのはダメですね。画竜点睛を欠く的に取り返しがつかないことしてしまった（またか）。
詳細は後述のフルプログラムを見てもらうとして、ここではメニューの階層構造を説明します。
目玉は、
- LIDARスキャン結果をOLEDに表示する
- Settingメニューでコンパスのキャリブレーションができる
- Settingメニューで保存済みデータを確認することができる
あたりかと思います。
メニュー制御
まずグローバル領域でメニュー項目を格納した構造体を作って、内容を定義します。構造体は、メニューの選択開始行、メニューの行数、メニューの表示内容（5行分の文字列）からなります。これがメニュー画面数だけ定義されてます（今回は12個）。
// メニューの階層構成
struct MENU_STRUCT
{
int state_start; // メニューの選択開始行
int state_num; // メニューの行数
String item[5]; // メニューの表示内容
};
// メニュー内容定義
MENU_STRUCT menu_items[12] =
{
{2, 3, {"MAIN MENU", " DISTANCE", " SCAN", " SETTTING", ""}}, // メインメニュー
{1, 2, {"", "", "", "", ""}},
{2, 3, {"SCAN MENU", " START", " SCAN MODE ", " RETURN", " ", }}, // スキャンメニュー
{1, 2, {"", "", "", "", ""}},
{1, 5, {" MODE1(11.3/stp)", " MODE2(5.6/stp)", " MODE3(2.8/stp)", " MODE4(0.7/stp)", " MODE5(0.35/stp)"}}, // スキャンモード選択
{1, 2, {"", "", "", "", ""}},
{2, 4, {"SETTING", " ANGLE CHANGE", " DATA DISPLAY", " COMPASS CALIB", " RETURN"}}, // セッティングメニュー
{2, 4, {"ANGLE:", " +10", " -10", " ZERO", " RETURN"}}, // LIDAR角度変更
{2, 3, {"DATA DISPLAY", " DISTANCE", " SCAN", " RETURN", " "}}, // 保存されたデータの確認
{1, 2, {"", "", "", "", ""}},
{1, 2, {"", "", "", "", ""}},
{2, 2, {"COMPASS CALIB", " START", " RETURN", " ", " "}} // コンパスのキャブレーション
};
メニュー間の遷移や処理はメインのloop()でやります。メインループ内では、
-
menu_refreshがtrueのときは表示にアップデートがあるということで画面を描画する -
UPボタン（DOWNボタン）が押されたらmenu_stateを一つ進める（戻す） -
menu_stateの位置には選択中を示す>を表示する -
GOボタンが押されたらmenu_goをtrueにして、各メニューに対応した処理をする
という制御になってます。詳細はフィルプログラム参照ですが、下にメニュー制御部分を抜き出したのを載せておきます。OLEDへのメニューの表示はDisplay_Menu()関数にまとめてます。
void loop()
{
int ret;
int menu_mode = MENU_MAIN; // メニュー階層の状態
int menu_state = 1; // メニュー内の状態
bool menu_go = false; // GOボタンが押された際
bool menu_refresh = true; // メニューを書き換えるかどうかのフラグ
bool scan_start = false;
bool SW_push = false;
// メインループ
while(true)
{
// ボタンが押された時の処理
if(digitalRead(PIN_SW1) == LOW) //UPボタン
{
if(SW_push == false)
{
SW_push = true;
menu_state--;
menu_refresh = true;
}
}
else if(digitalRead(PIN_SW2) == LOW) //DOWNボタン
{
if(SW_push == false)
{
SW_push = true;
menu_state++;
menu_refresh = true;
}
}
else if(digitalRead(PIN_SW3) == LOW) //GOボタン
{
if(SW_push == false)
{
SW_push = true;
menu_go = true;
menu_refresh = true;
}
}
else
{
SW_push = false;
}
// デイスプレイにメニューを表示＆処理
if(menu_refresh)
{
menu_refresh = false;
if(menu_state < 1) menu_state = menu_items[menu_mode].state_num;
if(menu_state > menu_items[menu_mode].state_num) menu_state = 1;
Display_Menu(menu_mode, menu_state); // メニュー表示
switch(menu_mode)
{
case MENU_MAIN: // メインメニュー
if(menu_go)
{
menu_go = false;
switch(menu_state)
{
case 1:
menu_mode = MENU_DISTANCE;
break;
case 2:
menu_mode = MENU_SCAN;
break;
case 3:
menu_mode = MENU_SETTING;
break;
}
menu_state = 1;
menu_refresh = true;
}
break;
case MENU_DISTANCE: // LIDAR 距離測定メニュー
------------以下ほかのメニュー処理へ
LIDARスキャン結果の表示
今回はスタンドアロンということで、LIDARのスキャン結果をOLEDに表示するようにしました。画面が128x64ドットしかないのでぎっちぎちです・・・。結果表示はDraw_Graph()関数です。
いや、もっと高解像度のカラー液晶ディスプレイを使う手もあるんですが、制御がSPIになってしまい別系統の配線をやらなきゃなんでまた負のスパイラルに・・・。
スマホからの制御
スマホからのWebページを使っての制御は基本的に前回と同じですが、サービスページを設けてデータのダウンロードなどができるように変更してます。あと、RTCの時刻セットですね。コンパスのキャリブレーションは今回は本体メニューからできるようにしたので、スマホでの制御は省きました。
サービスページでは、DOWNLOADボタンでファイルをダウンロード、LOADボタンでスキャン結果を読み込みます。メインページに戻れば結果が表示されます。DELETEボタンはファイル消去です（「ホントに削除する？」っていう確認付きです）。SET TIMEボタンは前述したようにスマホの時刻データをLIDARに送り、LIDAR側でRTCの時刻合わせをします。
7. 制御プログラム
もはや自分でも何がなんやらわからないぐらいに肥大化してしまい説明がさらにめんどくさくなったので、もう省きます。プログラム内のコメントで許してくだされ。
関数とプログラム構成
- ディスプレイ表示関係
Display_Text(int x, int y, String word, bool overwrite)：OLEDへテキスト表示
Display_Menu(int menuMode, int menuState)：メニューの表示
Draw_Graph_Frame()：スキャン結果グラフのフレーム描画
Draw_Graph()：スキャン結果グラフ表示 - ステッピングモーター関係
Motor_stop()：ステッピングモーターを停止
Motor_move(int step)：ステッピングモーターを指定ステップ進める
Motor_Zero()：ステッピングモーターの原点復帰 - コンパス、傾きセンサー
Compass()：コンパス＆加速度センサー測定
Compass_calib()：コンパスのキャリブレーション - LIDAR測定関係
LIDAR_measurement()：LIDAR 1点測定
angleSearch(float search_angle)：指定角度のデータ番号を検索する
LIDAR(void *pvParameters)：LIDAR測定（マルチタスク） - SPIFFSデータ保存
spiffsTransfer()：SPIFFSに保存されたデータをクライアントに送信
memoryData()：距離測定データをSPIFFSに保存
saveData()：LIDARスキャンデータをSPIFFSに保存 - RTC関係
setRTC()：RTCの時刻を設定 - Webページ
servicePage()：サービスページ表示
handleAccess()：メインページ処理
handleNotFound()：スマホからのリクエストにエラーがある場合
waitingClient(void *pvParameters)：Webサーバーのリクエスト待ち（マルチタスク） - メイン処理
setup()
loop()
プログラム
フルプログラム
/////////////////////////////////////////////////////////////////////////
// //
// LIDAR3 CONTROL Program ver2.8 2024/03/24 Inufox //
// ESP32 MH-LIVE Mini Kit //
// Lidar TW10S-UART //
// Stepping motor 28BYJ-48 + driver ULN2003 //
// OLED 128x64(SH1106) //
// Compass&Acc LSM303 //
// RTC DS3231 //
// //
/////////////////////////////////////////////////////////////////////////
#include <Arduino.h>
#include <Wire.h>
#include <math.h>
#include <Stepper.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <U8g2lib.h>
#include <WiFi.h>
#include <WiFiAP.h>
#include <WebServer.h>
#include <SPIFFS.h>
#include <RTClib.h>
#include <uri/UriBraces.h>
#define PIN_I2C_SDA 21
#define PIN_I2C_SCL 22
#define PIN_LED 2
///////////////////////////////////////////////
// 初期設定
///////////////////////////////////////////////
// 設定、データ保存用のファイル名
const String PROGRAM_VERSION = "2.8";
const String memoryname = "DISTANCE_MEMORY.txt";
const String settingfile = "SYSTEM_SETTING.ini";
const String calibfile = "SYSTEM_CALIBRATION.ini";
// WiFi & Web server
WebServer server(80);
const char* APssid = "TENOHIRA_LIDAR3"; // WiFiアクセスポイントのSSID
const char* APpassword = ""; // WiFiアクセスポイントのpassword
IPAddress ipadd(192,168,11,1); // WiFiアクセスポイントのIPアドレス
IPAddress subnet(255,255,255,0); // WiFiアクセスポイントのサブネットマスク
bool autorequest = false; // TrueならWebページが自動読み込みモードになる
// Display SSD1116 128x64(i2c address 0x3C)
U8G2_SH1106_128X64_NONAME_F_HW_I2C display(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);
// Accelerometer & Compass LSM303(i2c address Compass:0x1e/Acc:0x18)
Adafruit_LSM303_Accel_Unified LSM303acc = Adafruit_LSM303_Accel_Unified(54321);
Adafruit_LSM303_Mag_Unified LSM303mag = Adafruit_LSM303_Mag_Unified(12345);
struct COMPASS_CALIB
{
float AX, AY, AZ; // コンパス 振幅値
float M0X, M0Y, M0Z; // コンパス オフセット値
float declination; // コンパス 偏角
};
COMPASS_CALIB calib_data;
bool compass_ok; // コンパスが正常起動したらTrue
float accX, accY, accZ;
float magX, magY, magZ;
float elevation, azimuth;
// RTC DS3231(i2c address 0x68)
RTC_DS3231 rtc;
bool rtc_ok; // RTCが正常起動したらTrue
// LIDAR TW10S(UART)
#define PIN_LIDAR_Rx 17
#define PIN_LIDAR_Tx 16
#define PIN_LIDAR_Active 26 // EN端子にdigitalWriteで3.3Vをかけるとフリーズする → dacWriteで1.5Vをかける
// おそらく金属ビスのせいでサービス端子がショートし、初期にチェックモードに入るようになってしまった
// ENに3.3Vをかけるとサービスモードに入ったときに電流が流れすぎてフリーズすると推定
// LIDARの測定値変数
#define DISTANCE_OFFSET 4 // レーザー距離計のオフセット長（cm）；レンズ～回転中心までの長さ
#define MAX_DATA 520 // 最大測定点
volatile int LIDAR_ndata; // データ点数
volatile float LIDAR_currentdistance; // 現在の測定距離
volatile float LIDAR_angle[MAX_DATA]; // スキャン角度
volatile float LIDAR_distance[MAX_DATA]; // スキャン距離
// LIDARの制御用変数
#define LIDAR_TIMEOUT 1000 // レーザー距離計の応答待ちタイムアウト時間
#define LIDAR_WAIT 800 // サーボを動かしてから測定までの待ち時間（msec）
#define LIDAR_WAITING 0 // LIDARの状態定数（0=停止状態）
#define LIDAR_READY 1 // LIDARの状態定数（1=待機状態）
#define LIDAR_MEASURING 2 // LIDARの状態定数（2=測定中）
#define LIDAR_SCANNING 3 // LIDARの状態定数（3=スキャン中）
#define LIDAR_FALSE 99 // LIDARの起動失敗
volatile int lidar_state = LIDAR_WAITING; // LIDARの状態
int scanlevel = 3; // スキャン種類
int ang_start[] = {0, -32, -32, -32, -32}; // スキャン種類のパラメーター
int ang_end[] = {1024, 1056, 1056, 1056, 1056};
int ang_step[] = {64, 32, 16, 4, 2};
bool lidar_ok; // Lidarが正常起動したらTrue
bool tobesaved = false;
HardwareSerial UART_LIDAR(1);
// 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}; // Single measurement
uint8_t lidar_continuous[] = {0x01, 0x03, 0x00, 0x01, 0x00, 0x02, 0x95, 0xCB}; // Continuous measurement
uint8_t lidar_stop[] = {0x01, 0x03, 0x00, 0x0A, 0x00, 0x02, 0xE4, 0x09}; // Stop Continuous measurement
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
// ステッピングモーター 28BYJ-48 & ULN2003
#define PIN_MOTOR_1 18
#define PIN_MOTOR_2 19
#define PIN_MOTOR_3 23
#define PIN_MOTOR_4 5
#define PIN_SENSE 13 // 原点検出用ピン
#define ANGLE_PER_STEP 0.17578125 // 1ステップの角度=180/1024
#define ZERO_POINT 24 // 0検出から0°位置までのステップ数
#define STEP_360 2048 // 360°のステップ数
#define MAX_ANGLE 1089 // 最大角度（1024+64→191.25°）
#define MIN_ANGLE -65 // 最小角度（-64→11.25°）
#define MOTOR_SPEED 8 // ステッピングモーターの回転速度
volatile float motor_angle = 0; // モーターの現在角度（ステップ単位）
bool motor_ok; // モーターが正常起動したらTrue
Stepper Motor(STEP_360, PIN_MOTOR_1, PIN_MOTOR_3, PIN_MOTOR_2, PIN_MOTOR_4);
// スイッチ
#define PIN_SW1 27
#define PIN_SW2 25
#define PIN_SW3 32
#define PIN_COVER 15 // ふたの開閉検出スイッチ
///////////////////////////////////////////////
// 関数
///////////////////////////////////////////////
// OLEDへテキスト表示
void Display_Text(int x, int y, String word, bool overwrite)
{
if(overwrite) // Overwrite=trueの場合は1行削除して上書き
{
display.setCursor(0, 13*y-1);
display.print(" ");
}
display.setCursor(7*(x-1), 13*y-1);
display.print(word);
display.sendBuffer();
}
// メニュー関連の定義＆関数
// メニューの階層番号
#define MENU_MAIN 0
#define MENU_DISTANCE 1
#define MENU_SCAN 2
#define MENU_SCANNING 3
#define MENU_SCANLEVEL 4
#define MENU_SCANFINISH 5
#define MENU_SETTING 6
#define MENU_ANGLE 7
#define MENU_DATA 8
#define MENU_DATA_DISTANCE 9
#define MENU_DATA_SCAN 10
#define MENU_CALIB 11
// メニューの階層構成
struct MENU_STRUCT
{
int state_start; // メニューの選択開始行
int state_num; // メニューの行数
String item[5]; // メニューの表示内容
};
// メニュー内容定義
MENU_STRUCT menu_items[12] =
{
{2, 3, {"MAIN MENU", " DISTANCE", " SCAN", " SETTTING", ""}}, // メインメニュー
{1, 2, {"", "", "", "", ""}},
{2, 3, {"SCAN MENU", " START", " SCAN MODE ", " RETURN", " ", }}, // スキャンメニュー
{1, 2, {"", "", "", "", ""}},
{1, 5, {" MODE1(11.3/stp)", " MODE2(5.6/stp)", " MODE3(2.8/stp)", " MODE4(0.7/stp)", " MODE5(0.35/stp)"}}, // スキャンモード選択
{1, 2, {"", "", "", "", ""}},
{2, 4, {"SETTING", " ANGLE CHANGE", " DATA DISPLAY", " COMPASS CALIB", " RETURN"}}, // セッティングメニュー
{2, 4, {"ANGLE:", " +10", " -10", " ZERO", " RETURN"}}, // LIDAR角度変更
{2, 3, {"DATA DISPLAY", " DISTANCE", " SCAN", " RETURN", " "}}, // 保存されたデータの確認
{1, 2, {"", "", "", "", ""}},
{1, 2, {"", "", "", "", ""}},
{2, 2, {"COMPASS CALIB", " START", " RETURN", " ", " "}} // コンパスのキャブレーション
};
// メニューの表示
void Display_Menu(int menuMode, int menuState)
{
int count;
if(menu_items[menuMode].item[0] != "")
{
for(count = 1 ; count < 6 ; count++)
{
if(menu_items[menuMode].item[count-1] != "")
{
Display_Text(1, count, menu_items[menuMode].item[count-1], true);
}
}
Display_Text(1, menuState + menu_items[menuMode].state_start - 1 , " >", false);
}
}
// スキャングラフ フレーム描画
void Draw_Graph_Frame()
{
display.drawFrame(47, 1, 81, 51);
display.drawLine(47, 46, 127, 46);
display.drawLine(87, 1, 87, 51);
}
// スキャングラフ表示
void Draw_Graph()
{
int count;
for (count = 0; count < LIDAR_ndata; count++)
{
display.drawPixel(87 + int(LIDAR_distance[count] * cos(LIDAR_angle[count] * M_PI / 180)/10), 46 - int(LIDAR_distance[count] * sin(LIDAR_angle[count] * M_PI / 180)/10));
}
}
// ステッピングモーターを停止
void Motor_stop()
{
digitalWrite(PIN_MOTOR_1, LOW);
digitalWrite(PIN_MOTOR_2, LOW);
digitalWrite(PIN_MOTOR_3, LOW);
digitalWrite(PIN_MOTOR_4, LOW);
}
// ステッピングモーターを指定ステップ進める
int Motor_move(int step)
{
bool flag = false;
if(digitalRead(PIN_COVER) == HIGH)
{
if ((motor_angle + step >= MIN_ANGLE) && (motor_angle + step <= MAX_ANGLE))
{
Motor.step(-step); // 角度方向とステップ符号が逆なので注意
motor_angle = motor_angle + step;
flag = true;
}
}
return flag;
}
// ステッピングモーターの原点復帰
int Motor_Zero()
{
long timer = millis();
bool flag = true;
lidar_state = LIDAR_READY;
motor_angle = 0;
// 原点への荒移動
Serial.println("Finding origin");
Display_Text(0, 1, "Finding origin",true);
while(digitalRead(PIN_SENSE) == HIGH)
{
if(millis() > timer + 30000) // タイムアウト30秒でエラー終了
{
flag = false;
break;
}
motor_angle = 0;
Motor_move(-4);
delay(50);
}
delay(200);
if(flag == true)
{
Serial.println("Qrigin found");
Display_Text(1, 1, "Origin found", true);
// 一旦プラス方向に戻す
Motor_move(8);
delay(200);
// 細かく原点まで戻す
timer = millis();
while(digitalRead(PIN_SENSE) == HIGH)
{
if(millis() > timer + 5000) // タイムアウト5秒でエラー終了
{
flag = false;
break;
}
motor_angle = 0;
Motor_move(-1);
delay(50);
}
}
if(flag == true)
{
// 0°位置まで移動
Motor_move(ZERO_POINT);
motor_angle = 0;
Serial.println("Motor at 0deg");
Display_Text(1, 1, "Motor at 0deg",true);
}
else
{
Serial.println("Zero return error");
Display_Text(1, 1, "Zero return error",true);
}
Motor_stop();
delay(500);
return flag;
}
// コンパス＆加速度センサー測定
int Compass()
{
sensors_event_t event;
// Accel UNIT:m/s^2
LSM303acc.getEvent(&event);
accX = event.acceleration.x;
accY = event.acceleration.y;
accZ = event.acceleration.z;
// Compass UNIT:uT
LSM303mag.getEvent(&event);
magX = event.magnetic.x;
magY = event.magnetic.y;
magZ = event.magnetic.z;
elevation = 180 * atan2(accY, accZ) / PI;
azimuth = atan2(-(magY - calib_data.M0Y) / calib_data.AY, -(magX - calib_data.M0X) / calib_data.AX) * 180 / PI + calib_data.declination;
return true;
}
// コンパスのキャリブレーション
int Compass_calib()
{
DateTime rtctime;
char buf[40];
float cDeclination = -8.0; // 日本での標準的な磁気偏角
float cM0X, cM0Y, cM0Z;
float cAX, cAY, cAZ;
float max_magX = -1000;
float max_magY = -1000;
float max_magZ = -1000;
float min_magX = 1000;
float min_magY = 1000;
float min_magZ = 1000;
// SW3が押されるまで測定値の最大・最小を取り続ける
while(digitalRead(PIN_SW3) == HIGH)
{
Compass();
max_magX = max(magX, max_magX);
max_magY = max(magY, max_magY);
max_magZ = max(magZ, max_magZ);
min_magX = min(magX, min_magX);
min_magY = min(magY, min_magY);
min_magZ = min(magZ, min_magZ);
cAX = 0.5 * (max_magX - min_magX);
cAY = 0.5 * (max_magY - min_magY);
cAZ = 0.5 * (max_magZ - min_magZ);
cM0X = 0.5 * (max_magX + min_magX);
cM0Y = 0.5 * (max_magY + min_magY);
cM0Z = 0.5 * (max_magZ + min_magZ);
azimuth = atan2(-(magY - cM0Y) / cAY, -(magX - cM0X) / cAX) * 180 / PI + cDeclination;
Display_Text(1, 1, "X " + String(max_magX, 2) + " " + String(min_magX, 2), true);
Display_Text(1, 2, "Y " + String(max_magY, 2) + " " + String(min_magY, 2), true);
Display_Text(1, 3, "Z " + String(max_magZ, 2) + " " + String(min_magZ, 2), true);
Display_Text(1, 4, "AZ " + String(azimuth, 1), true);
delay(50);
}
// SW3が押された時点の方向を北とする
Display_Text(1, 5, "Turn north & GO", true);
delay(500);
while(digitalRead(PIN_SW3) == HIGH) {}
// キャリブレーション値設定
calib_data.AX = cAX;
calib_data.AY = cAY;
calib_data.AZ = cAZ;
calib_data.M0X = cM0X;
calib_data.M0Y = cM0Y;
calib_data.M0Z = cM0Z;
Compass();
azimuth = atan2(-(magY - cM0Y) / cAY, -(magX - cM0X) / cAX) * 180 / PI + cDeclination;
calib_data.declination = -(azimuth - cDeclination); // キャリブレーション停止時の方向を北とする
// SPIFFSへキャリブレーション値を書き込み
File fp = SPIFFS.open("/" + calibfile, FILE_WRITE);
rtctime = rtc.now(); // 時刻取得
sprintf(buf, "Calibration date %04d/%02d/%02d,%02d:%02d:%02d", rtctime.year(), rtctime.month(), rtctime.day(), rtctime.hour(), rtctime.minute(), rtctime.second());
fp.println(buf);
fp.println(calib_data.AX);
fp.println(calib_data.AY);
fp.println(calib_data.AZ);
fp.println(calib_data.M0X);
fp.println(calib_data.M0Y);
fp.println(calib_data.M0Z);
fp.println(calib_data.declination);
fp.close();
Serial.println("Compass calibration data is saved");
return true;
}
// LIDAR測定
float LIDAR_measurement()
{
int count;
long waitbegin;
bool timeout = true;
float distance;
char buf;
char data[20];
while(UART_LIDAR.available() > 0) // 距離センサーからの受信バッファを一旦空にする
{
buf = UART_LIDAR.read();
}
waitbegin = millis();
while(UART_LIDAR.available() < 9) // 距離センサーからの測定データが9バイト以上になるまで待つ
{
if(millis()-waitbegin > LIDAR_TIMEOUT) // タイムアウトになったらエラー
{
Serial.println("TIMEOUT");
distance = -1;
timeout = false;
break;
}
delay(5);
}
// 測定データ処理開始
if(timeout == true)
{
if(UART_LIDAR.find(lidar_header, 3) == true)
{
for (count = 0; count < 6; count++)
{
data[count] = 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; // 測定値エラー
}
}
return distance;
}
// 指定角度のデータ番号を検索する
int angleSearch(float search_angle)
{
int count = 0;
while(LIDAR_angle[count] <= search_angle)
{
count++;
if(count > MAX_DATA || count > LIDAR_ndata)
{
break;
}
}
return count - 1;
}
/////////////////////////////////////////////////////////////////////////
// SIPFFS処理
/////////////////////////////////////////////////////////////////////////
// 距離測定データをSPIFFSに保存
void memoryData()
{
int ret;
char buf[20];
DateTime rtctime;
File fp;
ret = Compass(); // コンパスデータ取得
rtctime = rtc.now(); // 時刻取得
sprintf(buf, "%04d/%02d/%02d,%02d:%02d:%02d", rtctime.year(), rtctime.month(), rtctime.day(), rtctime.hour(), rtctime.minute(), rtctime.second());
// SPIFFSへ書き込み
fp = SPIFFS.open("/" + memoryname, FILE_APPEND);
fp.print(buf);fp.print(",");
fp.print(motor_angle*ANGLE_PER_STEP);fp.print(",");
fp.print(LIDAR_currentdistance);fp.print(",");
fp.print(azimuth);fp.print(",");
fp.print(magX);fp.print(",");
fp.print(magY);fp.print(",");
fp.print(magZ);fp.print(",");
fp.print(accX);fp.print(",");
fp.print(accY);fp.print(",");
fp.println(accZ);
fp.close();
Serial.println("Distance data is saved");
}
// LIDARスキャンデータをSPIFFSに保存
void saveData()
{
int ret;
int count1;
char buf[20];
DateTime rtctime;
File fp;
ret = Compass(); // コンパスデータ取得
rtctime = rtc.now(); // 時刻取得
sprintf(buf, "%04d%02d%02d_%02d%02d%02d", rtctime.year(), rtctime.month(), rtctime.day(), rtctime.hour(), rtctime.minute(), rtctime.second());
// SPIFFSへ書き込み
fp = SPIFFS.open("/SCAN" + String(buf) + ".txt", FILE_WRITE);
// ヘッダー
fp.println("LIDAR3 data start");
fp.println("Control program v" + PROGRAM_VERSION);
fp.println("Scan data");
sprintf(buf, "%04d/%02d/%02d,%02d:%02d:%02d", rtctime.year(), rtctime.month(), rtctime.day(), rtctime.hour(), rtctime.minute(), rtctime.second());
fp.println(buf);
fp.println("Data number=" + String(LIDAR_ndata));
fp.println("Tilt(x y z)=" + String(accX,3) + "," + String(accY,3) + "," + String(accZ,3));
fp.println("Compass(x y z)=" + String(magX,3) + "," + String(magY,3) + "," + String(magZ,3));
fp.println("Azimuth=" + String(azimuth,1));
fp.println("angle,distance,x,z");
// スキャンデータ
for (count1 = 0; count1 < LIDAR_ndata; count1++)
{
fp.print(String(LIDAR_angle[count1],4) + "," + String(LIDAR_distance[count1], 1) + ",");
fp.print(String(LIDAR_distance[count1] * cos(LIDAR_angle[count1] * M_PI / 180), 1) + ",");
fp.println(String(LIDAR_distance[count1] * sin(LIDAR_angle[count1] * M_PI / 180), 1));
}
fp.close();
}
/////////////////////////////////////////////////////////////////////////
// HTML処理
/////////////////////////////////////////////////////////////////////////
// SPIFFSに保存されたデータをクライアントに送信
// https://johnwargo.com/posts/2023/arduino-esp32-web-server-on-a-processor-core/
void spiffsTransfer()
{
String datafile;
String fname = server.pathArg(0);
Serial.println("SPIFFS data transfer");
Serial.println(fname);
File fp = SPIFFS.open("/" + fname, FILE_READ);
while(fp.available())
{
datafile += fp.readStringUntil('\n') + "\n";
}
Serial.println(datafile);
server.send(200, "text/plain", datafile);
}
// RTCの時刻を設定
void setRTC()
{
int count1;
String datetime;
int current_year, current_month, current_day;
int current_hour, current_minute, current_second;
// スマホから送られてきた日付データを取得
datetime = server.arg("setdatetime");
current_year = datetime.substring(0, 4).toInt();
current_month = datetime.substring(4, 6).toInt();
current_day = datetime.substring(6, 8).toInt();
current_hour = datetime.substring(9, 11).toInt();
current_minute = datetime.substring(11, 13).toInt();
current_second = datetime.substring(13, 15).toInt();
// RTCを合わせる
rtc.adjust(DateTime(current_year, current_month, current_day, current_hour, current_minute, current_second));
Serial.println("Set RTC time");
Display_Text(1, 1, "Set RTC Time", true);
}
// サービスページ
void servicePage()
{
int count;
String html, fname, message;
File root;
File fp;
Serial.println("Service page");
if(server.args()>0)
{
Serial.println("Argument: " + server.argName(0) + "=" + server.arg(server.argName(0)));
}
if (server.hasArg("delete")) // SPIFFSファイルの消去
{
fname = server.arg("delete");
Serial.println("Delete: " + fname);
SPIFFS.remove("/" + fname);
message = "File: " + fname + " in deleted";
}
if (server.hasArg("setdatetime")) // RTC時刻をセット
{
setRTC();
message = "RTC time is set";
}
if (server.hasArg("load")) // SPIFFSファイルからスキャンデータを読み出し
{
fname = server.arg("load");
fp = SPIFFS.open("/" + fname);
while(!fp.readStringUntil('=').endsWith("number")){}
LIDAR_ndata = fp.readStringUntil('\n').toInt();
fp.readStringUntil('\n');
for(count = 0; count < LIDAR_ndata; count++)
{
LIDAR_angle[count] = fp.readStringUntil(',').toFloat();
LIDAR_distance[count] = fp.readStringUntil(',').toFloat();
fp.readStringUntil('\n');
}
fp.close();
message = "Data: " + fname + " is loaded<br>(" + LIDAR_ndata + "points)";
}
Serial.println("Send service page to client");
// HTML
// ヘッダー
html = "<!DOCTYPE html>";
html += "<html>";
html += "<head>";
html += "<title>LIDAR3 Service Page</title>";
// スクリプト
html += "<script type = \"text/javascript\">";
html += "function DeleteCheck(fname){"; // SPIFFSファイル消去の確認
html += " return confirm('Delete ' + fname + ' ?');}";
html += "function setRTC(){"; // RTCセット
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 form = document.createElement('form');";
html += " form.action = '/service';";
html += " form.method = 'POST';";
html += " var que = document.createElement('input');";
html += " que.value = ftoday;";
html += " que.name = 'setdatetime';";
html += " form.appendChild(que);";
html += " document.body.appendChild(form);";
html += " form.submit();}";
html += "</script>";
html += "</head>";
// HTMLページスタート
html += "<body style=\"font-size:40px; color:white; background-color:dimgray\">";
html += "<div style=\"font-size:120%; color:coral\">LIDAR3 Service Page</div>";
html += "Tenohira Lidar 3 v" + PROGRAM_VERSION;
html += "<hr>";
// SPIFFS内のファイルリストを表示
html += "<div style=\"color:deepskyblue\">SPIFFS Data Files</div>";
html += "<table border=\"1\">";
root = SPIFFS.open("/");
// システムファイル
fp = root.openNextFile();
while(fp)
{
fname = String(fp.name());
if(fname.startsWith("SYSTEM"))
{
html += "<TR><TD>" + fname + "</TD>";
html += "<TD><a href=\"/LIDAR_" + fname + "\" download><button style=\"font-size:100%\">DOWNLOAD</button></a>";
html += "<TD></TD>";
html += "<TD><form method=\"post\" onSubmit=\"return DeleteCheck('" + fname + "')\"><button style=\"font-size:100%\" type=\"submit\" name=\"delete\" value=\"" + fname + "\">DELETE</button></form></TD></TR>";
}
fp = root.openNextFile();
}
fp.close();
// 距離データファイル
if (SPIFFS.exists("/" + memoryname))
{
html += "<TR><TD>" + memoryname + "</TD>";
html += "<TD><a href=\"/LIDAR_" + memoryname + "\" download><button style=\"font-size:100%\">DOWNLOAD</button></a><TD></TD>";
html += "<TD><form method=\"post\" onSubmit=\"return DeleteCheck('" + memoryname + "')\"><button style=\"font-size:100%\" type=\"submit\" name=\"delete\" value=\"" + memoryname + "\">DELETE</button></form></TD></TR>";
}
root.rewindDirectory();
fp = root.openNextFile();
// スキャンデータファイル
while(fp)
{
fname = String(fp.name());
if(fname.startsWith("SCAN") == true)
{
html += "<TR><TD>" + fname + "</TD>";
html += "<TD><a href=\"/LIDAR_" + fname + "\" download><button style=\"font-size:100%\">DOWNLOAD</button></a>";
html += "<TD><form method=\"post\"><button style=\"font-size:100%\" type=\"submit\" name=\"load\" value=\"" + fname + "\">LOAD</button></form></TD>";
html += "<TD><form method=\"post\" onSubmit=\"return DeleteCheck('" + fname + "')\"><button style=\"font-size:100%\" type=\"submit\" name=\"delete\" value=\"" + fname + "\">DELETE</button></form></TD></TR>";
}
fp = root.openNextFile();
}
fp.close();
root.close();
html += "</table>";
html += "<hr>";
html += "<input style =\"font-size:100%\" type=\"button\" value=\"SET TIME\" onclick=\"setRTC()\">";
html += "<hr>";
html += message;
html += "<hr>";
html += "<a href=\"/\"><button style=\"font-size:100%\">MAIN PAGE</button></a>";
html += "</body></html>";
server.send(200, "text/html", html);
}
// メインページ処理
void handleAccess()
{
int count1;
String html;
float angle_target;
Serial.println("Main page");
if(server.args()>0)
{
Serial.println("Argument: " + server.argName(0) + "=" + server.arg(server.argName(0)));
}
// 距離測定／スキャンの場合
if (server.hasArg("scan"))
{
if(digitalRead(PIN_COVER) == HIGH)
{
if(server.arg("scan") == "0") // Scan0（角度を変えずに連続測定）
{
lidar_state = LIDAR_MEASURING;
}
else
{
// 1（0～180°まで11.25°ステップで測定） 2（-5.6～185.6°まで5.6°ステップで測定）
// 3（-5.6～185.6°まで2.8°ステップで測定） 4（-5.6～185.6°まで0.7°ステップで測定）
// 5（-5.6～185.6°まで0.35°ステップで測定）
scanlevel = server.arg("scan").toInt();
lidar_state = LIDAR_SCANNING;
}
autorequest = true;
}
}
// コマンドの場合
if (server.hasArg("command"))
{
if (server.arg("command") == "memory") // 現在の測定データをメモリーに記憶
{
memoryData();
}
if (server.arg("command") == "stop") lidar_state = LIDAR_READY; // STOPコマンド
}
// LIDAR角度変更の場合
if (server.hasArg("angle"))
{
if(digitalRead(PIN_COVER) == HIGH)
{
if (server.arg("angle") == "zero") // 原点復帰
{
Motor_Zero();
}
else // 角度設定
{
lidar_state = LIDAR_READY;
angle_target = server.arg("angle").toInt();
Motor_move(int(angle_target/ANGLE_PER_STEP - motor_angle));
Motor_stop();
}
}
}
if(lidar_state == LIDAR_READY) autorequest = false;
Serial.println("Send control page to client");
// HTML
// ヘッダー
html = "<!DOCTYPE html>";
html += "<html>";
html += "<head><meta charset=\"utf-8\">";
if(autorequest == true) // スキャン中はページを自動リフレッシュ
{
html += "<meta http-equiv=\"refresh\" content=\"2; URL=/\">";
}
html += "<title>LIDAR3</title>";
html += "</head>";
html += "<body onload=\"draw();\" style=\"font-size:50px; color:white; text-align:center; background-color:dimgray\">";
html += "<div style=\"font-size:120%; color:coral\">LIDAR3 CONTROL</div><br>";
// 測定データ表示
html += "<table align=\"center\" border=\"1\" bordercolor=\"white\">";
html += "<tr align=\"left\"><td>ANGLE</td>";
html += "<td>" + String(motor_angle*ANGLE_PER_STEP,1) + "°</td>";
html += "<td>DISTANCE</td>";
html += "<td>" + String(LIDAR_currentdistance, 1) + "cm</td></tr>";
html += "<tr align=\"left\"><td></td><td>0°</td><td></td><td>";
html += String(LIDAR_distance[angleSearch(0)], 1) + "cm</td></tr>";
html += "<tr align=\"left\"><td></td><td>90°</td><td></td><td>";
html += String(LIDAR_distance[angleSearch(90)], 1) + "cm</td></tr>";
html += "<tr align=\"left\"><td></td><td>180°</td><td></td><td>";
html += String(LIDAR_distance[angleSearch(180)], 1) + "cm</td></tr>";
html += "</table><br>";
// LIDARコントロール
html += "<table align=\"center\" border=\"3\" bordercolor=\"white\" width=\"90%\">";
html += "<tr align=\"left\"><td>DISTANCE</td>";
html += "<td><div style =\"display:inline-flex\">";
html += "<form method=\"post\"><button style=\"font-size:100%\" type=\"submit\" name=\"scan\" value=\"0\">START</button></form>";
html += "  <form method=\"post\"><button style=\"font-size:100%\" type=\"submit\" name=\"command\" value=\"memory\">MEMORY</button></form>";
if (server.arg("command") == "memory")
{
html += " *"; // 距離データ保存ボタンが押されたら「*」を表示
}
html += "</div></td></tr>";
html += "<tr align=\"left\"><td>SCAN</td>";
html += "<td><form method=\"post\"> <input style =\"font-size:100%\" type=\"submit\" value=\"START\">";
html += " <select style =\"font-size:100%\" name=\"scan\">";
html += "<option value=\"1\"";
if(scanlevel == 1) html += " selected"; // スキャンレベル設定に合わせて表示を変える
html += ">11.3°/step</option>";
html += "<option value=\"2\"";
if(scanlevel == 2) html += " selected";
html += ">5.6°/step</option>";
html += "<option value=\"3\"";
if(scanlevel == 3) html += " selected";
html += ">2.8°/step</option>";
html += "<option value=\"4\"";
if(scanlevel == 4) html += " selected";
html += ">0.7°/step</option>";
html += "<option value=\"5\"";
if(scanlevel == 5) html += " selected";
html += "> 0.35°/step</option></select></form></td></tr>";
// ステッピングモーターの角度設定
html += "<tr align=\"left\"><td>ANGLE</td>";
html += "<td><div style=\"display:inline-flex\">";
html += "<form method=\"post\"><input style=\"font-size:100%;\" type=\"submit\" value=\"SET\">";
html += " <input style=\"font-size:100%;width:3em\" type=\"number\" min=\"-5\" max=\"185\" name=\"angle\" value=\"";
html += String(motor_angle*ANGLE_PER_STEP,1) + "\">°</form>";
html += " <form method=\"post\" style=\"display: inline\"> <button style=\"font-size:100%\" type=\"submit\" name=\"angle\" value=\"zero\">ZERO</button></form></div></td></tr>";
// STOP & サービスメニュー
html += "<tr align=\"center\"><td colspan=\"2\"><div style=\"display:inline-flex\">";
html += "<form method=\"post\"><button style =\"font-size:100%\" type=\"submit\" name=\"command\" value=\"stop\">STOP</button></form>";
html += "  <a href=\"/service\"><button style=\"font-size:100%\">SERVICE MENU</button></a></div></td></tr>";
html += "</table>";
// スキャングラフ描画（キャンバス機能を使う）
html += "<div style=\"color:deepskyblue\"><br>SCAN RESULT (1m/div)</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.lineWidth = 4;";
html += "ctx.strokeStyle = \"white\";";
html += "ctx.strokeRect(20, 0, 960, 520);";
html += "ctx.strokeRect(20, 0, 480, 520);";
html += "ctx.strokeRect(20, 0, 960, 500);";
html += "ctx.lineWidth = 1;";
html += "ctx.strokeRect(100, 0, 100, 520);";
html += "ctx.strokeRect(300, 0, 100, 520);";
html += "ctx.strokeRect(600, 0, 100, 520);";
html += "ctx.strokeRect(800, 0, 100, 520);";
html += "ctx.strokeRect(20, 100, 960, 100);";
html += "ctx.strokeRect(20, 300, 960, 100);";
// 測定結果表示
html += "ctx.fillStyle = \"mistyrose\";";
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 handleNotFound()
{
server.send(404, "text/plain", "Not Found");
}
// Webサーバーのリクエスト待ち（マルチタスク）
void waitingClient(void *pvParameters)
{
while(true)
{
server.handleClient();
delay(50);
}
}
/////////////////////////////////////////////////////////////////////////
// LIDAR測定
// マルチタスクで常時稼働しています
// Main側でlidar_stateをLIDAR_MEASURINGに変えると連続測定開始
// LIDAR_SCANNINGにするとスキャン開始
/////////////////////////////////////////////////////////////////////////
void LIDAR(void *pvParameters)
{
int current_angle;
float original_angle = motor_angle;
while(true)
{
// 連続測定開始
if(lidar_state == LIDAR_MEASURING)
{
digitalWrite(PIN_LED, HIGH);
UART_LIDAR.write(lidar_continuous, sizeof(lidar_continuous)); // LIDARアクティブ
// 連続測定
while(lidar_state == LIDAR_MEASURING)
{
LIDAR_currentdistance = LIDAR_measurement();
delay(300);
}
UART_LIDAR.write(lidar_stop, sizeof(lidar_stop));
digitalWrite(PIN_LED, LOW);
}
// スキャン開始
if(lidar_state == LIDAR_SCANNING)
{
digitalWrite(PIN_LED, HIGH);
UART_LIDAR.write(lidar_continuous, sizeof(lidar_continuous)); // LIDARアクティブ
current_angle = ang_start[scanlevel-1];
LIDAR_ndata = 0;
// スキャン
while(current_angle <= ang_end[scanlevel-1] && lidar_state == LIDAR_SCANNING)
{
Motor_move(current_angle - motor_angle);
delay(LIDAR_WAIT); // LIDARの測定待ち
LIDAR_currentdistance = LIDAR_measurement();
LIDAR_angle[LIDAR_ndata] = current_angle * ANGLE_PER_STEP;
LIDAR_distance[LIDAR_ndata] = LIDAR_currentdistance;
Serial.println(String(current_angle * ANGLE_PER_STEP, 1) + "," + String(LIDAR_currentdistance, 1));
current_angle += ang_step[scanlevel-1];
LIDAR_ndata++;
}
if(lidar_state == LIDAR_SCANNING) // 正常終了
{
Serial.println("Data num = " + String(LIDAR_ndata));
tobesaved = true;
}
UART_LIDAR.write(lidar_stop, sizeof(lidar_stop));
Motor_move(original_angle - motor_angle);
Motor_stop();
lidar_state = LIDAR_READY;
digitalWrite(PIN_LED, LOW);
}
delay(100);
}
}
/////////////////////////////////////////////////////////////////////////
// Setup
/////////////////////////////////////////////////////////////////////////
void setup()
{
int timer;
lidar_ok = false;
compass_ok = false;
motor_ok = false;
rtc_ok = false;
Serial.begin(115200);
Serial.println("Tenohira Lidar 3 v" + PROGRAM_VERSION);
Serial.println("Initializing");
pinMode(PIN_LED, OUTPUT);
digitalWrite(PIN_LED, LOW);
Wire.begin(PIN_I2C_SDA,PIN_I2C_SCL);
Serial.println("I2C OK");
// SPIFFS初期化
File fp;
SPIFFS.begin();
Serial.println("SPIFFS OK");
delay(200);
if (!SPIFFS.exists("/" + settingfile)) // fp = SPIFFS.open("/lidar_setting.ini", FILE_READ)だとファイルがなくてもTrueになる
{
// セッティングファイルがない場合はSPIFFSをフォーマットする
SPIFFS.format();
Serial.println("Format SPIFFS");
delay(1000);
}
// セッティングファイルの読み込み（現状はセッティングファイルにはバージョンが入ってるだけ）
fp = SPIFFS.open("/" + settingfile, FILE_READ);
if(fp.readStringUntil('\n') != "Tenohira Lidar 3 v" + PROGRAM_VERSION) // ヘッダーがコンパイル時と違う場合は書き換え
{
fp = SPIFFS.open("/" + settingfile, FILE_WRITE);
fp.println("Tenohira Lidar 3 v" + PROGRAM_VERSION);
fp.print("Compile date: ");
fp.print(__DATE__);
fp.print(" ");
fp.println(__TIME__);
Serial.println("Setting file updated");
delay(200);
}
else
{
Serial.println("Setting file OK");
Serial.println(fp.readStringUntil('\n'));
Serial.println(fp.readStringUntil('\n'));
delay(200);
}
fp.close();
// 距離データの保存ファイルがない場合は新規作成
if (!SPIFFS.exists("/" + memoryname))
{
fp = SPIFFS.open("/" + memoryname, FILE_WRITE);
fp.println("LIDAR3 distance data");
fp.println("Control program v" + PROGRAM_VERSION);
fp.println("DATE,TIME,angle,distance,azimuth,magX,magY,magZ,accX,accY,accZ");
fp.close();
Serial.println("Distance data file created");
delay(200);
}
// Display初期化
display.begin();
display.setContrast(255);
display.setFlipMode(1); // 0:0deg, 1:180deg
display.setFont(u8g2_font_t0_13_mf);
display.clearBuffer();
Serial.println("Display OK");
Display_Text(1, 3,"LIDAR 3 v" + PROGRAM_VERSION, true);
Display_Text(1, 1, "Display OK", true);
delay(200);
// WiFi
Serial.println("Starting WiFi AP");
WiFi.softAP(APssid, APpassword);
delay(200);
WiFi.softAPConfig(ipadd, ipadd, subnet);
IPAddress esp32IP = WiFi.softAPIP();
Serial.println("WiFi AP configured");
Serial.print("AP MAC: ");Serial.println(WiFi.softAPmacAddress());
Serial.print("IP address: "); Serial.println(esp32IP);
Display_Text(1, 1, "WiFi setup", true);
delay(200);
// Web server
server.on("/", handleAccess);
server.on("/service", servicePage);
server.on(UriBraces("/LIDAR_{}"), spiffsTransfer);
server.onNotFound(handleNotFound);
server.begin();
xTaskCreateUniversal(waitingClient, "waitingClient", 8192, NULL, 5, NULL, PRO_CPU_NUM ); // Web待ち受けタスク起動
Serial.println("Web server begin");
Display_Text(1, 1, "Web server setup", true);
delay(200);
// LSM303 Accelerometer & Compass
if (LSM303acc.begin() && LSM303mag.begin())
{
Serial.println("Compass OK");
Display_Text(1, 1, "Compass OK", true);
compass_ok = true;
// SPIFFSからキャリブレーション設定読み込み
if (SPIFFS.exists("/" + calibfile))
{
fp = SPIFFS.open("/" + calibfile, FILE_READ);
fp.readStringUntil('\n');
calib_data.AX = fp.readStringUntil('\n').toFloat();
calib_data.AY = fp.readStringUntil('\n').toFloat();
calib_data.AZ = fp.readStringUntil('\n').toFloat();
calib_data.M0X = fp.readStringUntil('\n').toFloat();
calib_data.M0Y = fp.readStringUntil('\n').toFloat();
calib_data.M0Z = fp.readStringUntil('\n').toFloat();
calib_data.declination = fp.readStringUntil('\n').toFloat();
Serial.println("Compass uses saved calibration value");
fp.close();
}
else
{
// 保存データが無い場合はデフォルト値を使用する
calib_data.AX = 50.0; calib_data.AY = 50.0; calib_data.AZ = 50.0;
calib_data.M0X = 0.0; calib_data.M0Y = 0.0; calib_data.M0Z = 0.0;
calib_data.declination = -8.0;
Serial.println("Compass uses default calibration value");
}
}
else
{
Serial.println("Compass NG");
Display_Text(1, 1, "Compass NG", true);
compass_ok = false;
delay(2000);
}
delay(200);
// RTC
if (rtc.begin())
{
Serial.println("RTC OK");
Display_Text(1, 1, "RTC OK", true);
rtc_ok = true;
}
else
{
Serial.println("RTC NG");
Display_Text(1, 1, "RTC NG", true);
rtc_ok = false;
}
delay(200);
// Stepping motor
pinMode(PIN_SENSE, INPUT);
pinMode(PIN_MOTOR_1, OUTPUT);
pinMode(PIN_MOTOR_2, OUTPUT);
pinMode(PIN_MOTOR_3, OUTPUT);
pinMode(PIN_MOTOR_4, OUTPUT);
Motor.setSpeed(MOTOR_SPEED);
if (Motor_Zero() == true)
{
Serial.println("Motor OK");
Display_Text(1, 1, "Motor OK", true);
motor_ok = true;
}
else
{
Serial.println("Motor NG");
Display_Text(1, 1, "Motor NG", true);
motor_ok = false;
delay(2000);
}
delay(200);
// Switch
pinMode(PIN_SW1, INPUT_PULLUP);
pinMode(PIN_SW2, INPUT_PULLUP);
pinMode(PIN_SW3, INPUT_PULLUP);
pinMode(PIN_COVER, INPUT_PULLUP);
Serial.println("SW OK");
Display_Text(1, 1, "SW OK", true);
delay(200);
// LIDAR
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をセットアップ
Serial.println("LIDAR Initializing");
Display_Text(1, 1, "LIDAR Initializing", true);
digitalWrite(PIN_LIDAR_Active, HIGH); // レーザー距離センサーをアクティブに
delay(200);
timer = millis();
lidar_ok = true;
while(UART_LIDAR.find(lidar_ready, sizeof(lidar_ready)) != true) // レーザー距離センサーが起動OKを返すまで待機
{
if (millis() > timer + 6000)
{
lidar_ok = false;
break;
}
delay(20);
}
if (lidar_ok == true)
{
Serial.println("LIDAR OK");
Display_Text(1, 1, "LIDAR OK", true);
lidar_state = LIDAR_READY;
xTaskCreateUniversal(LIDAR, "LIDAR", 8192, NULL, 5, NULL, APP_CPU_NUM); // LIDAR測定タスクを開始
}
else
{
Serial.println("LIDAR NG");
Display_Text(1, 1, "LIDAR NG", true);
lidar_state = LIDAR_FALSE;
delay(2000);
}
Serial.println("Initialize OK");
Display_Text(1, 1, "Initialize OK", true);
delay(500);
}
/////////////////////////////////////////////////////////////////////////
// Main loop
/////////////////////////////////////////////////////////////////////////
void loop()
{
int ret;
int menu_mode = MENU_MAIN; // メニュー階層の状態
int menu_state = 1; // メニュー内の状態
bool menu_go = false; // GOボタンが押された際
bool menu_refresh = true; // メニューを書き換えるかどうかのフラグ
bool scan_start = false;
bool SW_push = false;
int counter = 0;
char buf[18];
DateTime rtctime;
File fp, root;
String readdate, readtime; // 測定時刻
String readdistance; // 測定結果
String fname; // データのファイル名
int datacount;
// メインループ
while(true)
{
// ボタンが押された時の処理
if(digitalRead(PIN_SW1) == LOW) //UPボタン
{
if(SW_push == false)
{
SW_push = true;
menu_state--;
menu_refresh = true;
}
}
else if(digitalRead(PIN_SW2) == LOW) //DOWNボタン
{
if(SW_push == false)
{
SW_push = true;
menu_state++;
menu_refresh = true;
}
}
else if(digitalRead(PIN_SW3) == LOW) //GOボタン
{
if(SW_push == false)
{
SW_push = true;
menu_go = true;
}
}
else
{
SW_push = false;
}
// GOボタンが押された時の処理
if(menu_go)
{
menu_go = false;
menu_refresh = true;
switch(menu_mode)
{
case MENU_MAIN: // メインメニュー
if(menu_state == 1)
{
menu_mode = MENU_DISTANCE;
lidar_state = LIDAR_MEASURING; // 連続測定開始
}
if(menu_state == 2) menu_mode = MENU_SCAN;
if(menu_state == 3) menu_mode = MENU_SETTING;
menu_state = 1;
break;
case MENU_DISTANCE: // LIDAR 距離測定メニュー
switch(menu_state)
{
case 1:
lidar_state = LIDAR_READY;
menu_mode = MENU_MAIN;
break;
case 2:
memoryData();
Display_Text(1, 5, " Data stored", true);
delay(500);
break;
}
break;
case MENU_SCAN: // LIDAR スキャンメニュー
switch(menu_state)
{
case 1:
menu_mode = MENU_SCANNING;
lidar_state = LIDAR_SCANNING; // スキャン開始
menu_state = 1;
break;
case 2:
menu_mode = MENU_SCANLEVEL;
menu_state = scanlevel;
break;
case 3:
menu_mode = MENU_MAIN;
menu_state = 1;
break;
}
break;
case MENU_SCANNING: // LIDAR スキャン中
menu_mode = MENU_SCAN;
Display_Text(1, 5, " Scan Interrupted", true);
delay(1000);
break;
case MENU_SCANFINISH: // スキャン後待機
menu_mode = MENU_SCAN;
break;
case MENU_SCANLEVEL: // スキャン条件設定
scanlevel = menu_state;
menu_mode = MENU_SCAN;
menu_state = 2;
break;
case MENU_SETTING: // 設定メニュー
if(menu_state == 1) menu_mode = MENU_ANGLE;
if(menu_state == 2) menu_mode = MENU_DATA;
if(menu_state == 3) menu_mode = MENU_CALIB;
if(menu_state == 4) menu_mode = MENU_MAIN;
menu_state = 1;
break;
case MENU_ANGLE: // 角度変更
switch(menu_state)
{
case 1:
Motor_move(64); // LIDRAを11.25°進める
Motor_stop();
break;
case 2:
Motor_move(-64); // LIDRAを11.25°戻す
Motor_stop();
break;
case 3:
Motor_Zero(); // LIDRAをZERO位置に戻す
break;
case 4:
menu_mode = MENU_SETTING;
menu_state = 1;
break;
}
break;
case MENU_DATA: // データの読み出しメニュー
switch(menu_state)
{
case 1:
menu_mode = MENU_DATA_DISTANCE;
menu_go = true;
break;
case 2:
menu_mode = MENU_DATA_SCAN;
menu_go = true;
break;
case 3:
menu_mode = MENU_SETTING;
break;
}
menu_state = 1;
break;
case MENU_DATA_DISTANCE: // 距離データの読み出し＆表示
switch(menu_state)
{
case 1:
// SPIFFS保存データの読み出し＆表示
if(!fp.available())
{
fp = SPIFFS.open("/" + memoryname, FILE_READ);
for(datacount = 1 ; datacount < 4 ; datacount++ ) fp.readStringUntil('\n');
}
datacount = 1;
while(datacount < 5)
{
if(fp.available())
{
fp.readStringUntil('/');
readdate = fp.readStringUntil(',');
readtime = fp.readStringUntil(':') + ":" + fp.readStringUntil(':');
fp.readStringUntil(',');
fp.readStringUntil(',');
readdistance = String(fp.readStringUntil(',').toInt());
fp.readStringUntil('\n');
Display_Text(1, datacount, readdate + " " + readtime + " " + readdistance, true);
}
else
{
Display_Text(1, datacount, "# End of Data #", true);
for( datacount++ ; datacount < 5 ; datacount++ ) Display_Text(1, datacount, " ", true);
fp.close();
break;
}
datacount++;
}
menu_refresh = false;
break;
case 2:
fp.close();
menu_mode = MENU_DATA;
menu_state = 1;
break;
}
break;
case MENU_DATA_SCAN: // スキャンデータの読み出し＆表示
switch(menu_state)
{
case 1:
// SPIFFS保存データの読み出し＆表示
Display_Text(1, 1, "Date", true);
Display_Text(1, 2, " ", true);
Display_Text(1, 3, "Time", true);
Display_Text(1, 4, " ", true);
Draw_Graph_Frame();
if(!root) root = SPIFFS.open("/");
do
{
fp = root.openNextFile();
if(fp) fname = String(fp.name());
} while(fp && fname.startsWith("SCAN") == false);
if(fp)
{
for(datacount = 1; datacount < 4; datacount++) fp.readStringUntil('\n');
fp.readStringUntil('/');
readdate = fp.readStringUntil(',');
readtime = fp.readStringUntil(':') + ":" + fp.readStringUntil(':');
fp.readStringUntil('=');
LIDAR_ndata = fp.readStringUntil('\n').toInt();
for(datacount = 1; datacount < 5; datacount++) fp.readStringUntil('\n');
for(datacount = 0; datacount < LIDAR_ndata; datacount++)
{
LIDAR_angle[datacount] = fp.readStringUntil(',').toFloat();
LIDAR_distance[datacount] = fp.readStringUntil(',').toFloat();
fp.readStringUntil('\n');
}
fp.close();
Display_Text(2, 2, readdate, false);
Display_Text(2, 4, readtime, false);
Draw_Graph();
}
else
{
Display_Text(1, 2, "# End of Data #", false);
delay(500);
root.rewindDirectory();
}
break;
case 2:
fp.close();
root.close();
menu_mode = MENU_DATA;
menu_state = 1;
break;
}
break;
case MENU_CALIB: // コンパスのキャリブレーション
switch(menu_state)
{
case 1:
Display_Text(1, 5, " >FINISH & SAVE", true);
Compass_calib();
Display_Text(1, 5, " Calib. Finished", true);
delay(2000);
menu_state = 2;
break;
case 2:
menu_mode = MENU_SETTING;
menu_state = 1;
break;
}
break;
}
}
// デイスプレイにメニューを表示＆処理
if(menu_refresh)
{
menu_refresh = false;
if(menu_state < 1) menu_state = menu_items[menu_mode].state_num;
if(menu_state > menu_items[menu_mode].state_num) menu_state = 1;
Display_Menu(menu_mode, menu_state); // メニュー表示
// 付加表示
switch(menu_mode)
{
case MENU_DISTANCE: // LIDAR 距離測定メニュー
if(menu_state == 1) Display_Text(1, 5, " >STOP SAVE", true);
if(menu_state == 2) Display_Text(1, 5, " STOP >SAVE", true);
case MENU_SCAN: // LIDAR スキャンメニュー
Display_Text(13, 3, String(scanlevel), false);
break;
case MENU_SCANNING: // LIDAR スキャン中
menu_state = 1;
Display_Text(1, 5, " >STOP", true);
break;
case MENU_SCANFINISH: // スキャン後待機
menu_state = 1;
Display_Text(1, 5, " >RETURN", true);
break;
case MENU_ANGLE: // 角度変更
Display_Text(8, 1, String(motor_angle*ANGLE_PER_STEP,1) + char(176) + " ", false);
break;
case MENU_DATA_DISTANCE: // 距離データの読み出し＆表示
if(menu_state == 1) Display_Text(1, 5, " >NEXT RETURN", true);
if(menu_state == 2) Display_Text(1, 5, " NEXT >RETURN", true);
break;
case MENU_DATA_SCAN: // スキャンデータの読み出し＆表示
if(menu_state == 1) Display_Text(1, 5, " >NEXT RETURN", true);
if(menu_state == 2) Display_Text(1, 5, " NEXT >RETURN", true);
break;
}
}
// ループ5回に１回ここを処理
if(counter > 5)
{
if(menu_mode == MENU_MAIN) // メインメニューの場合は時刻を表示
{
rtctime = rtc.now();
sprintf(buf, " %02d/%02d %02d:%02d:%02d", rtctime.month(), rtctime.day(), rtctime.hour(), rtctime.minute(), rtctime.second());
Display_Text(1, 5, buf, true);
}
if(lidar_state == LIDAR_MEASURING) // LIDAR測定中の場合は測定値を表示
{
ret = Compass();
Serial.println("DISTANCE=" + String(LIDAR_currentdistance, 1) + "cm");
Display_Text(1, 1, "LIDAR MEASURING", true);
Display_Text(1, 2, "DIST=" + String(LIDAR_currentdistance,1) + "cm", true);
Display_Text(1, 3, "DT/HT=" + String(LIDAR_currentdistance * cos(elevation / 180 * PI), 1) + "/" + String(LIDAR_currentdistance * sin(elevation / 180 * PI), 1), true);
Display_Text(1, 4, "EL=" + String(elevation,0) + char(176) + "/AZ=" + String(azimuth,0) + char(176), true);
}
if(lidar_state == LIDAR_SCANNING) // LIDARスキャン中の場合は測定値を表示
{
if(scan_start == false) // 1回目はグラフ枠を描く
{
scan_start = true;
Display_Text(1, 1, "DIST", true);
Display_Text(1, 2, " ", true);
Display_Text(1, 3, "ANGLE", true);
Display_Text(1, 4, " ", true);
Draw_Graph_Frame();
}
sprintf(buf, " %3dcm", int(LIDAR_currentdistance));
Display_Text(1, 2, String(buf), false);
sprintf(buf, " %3d", int(motor_angle * ANGLE_PER_STEP));
Display_Text(1, 4, String(buf) + char(176), false);
Draw_Graph();
}
counter = 0;
}
counter++;
// LIDARスキャンが正常終了したらデータをセーブ
if(lidar_state != LIDAR_SCANNING && scan_start == true)
{
scan_start = false;
menu_mode = MENU_SCANFINISH;
menu_refresh = true;
if(tobesaved == true)
{
tobesaved = false;
saveData();
Serial.println("Scan data saved");
Display_Text(1, 5, "Scan data saved", true);
delay(1000);
}
}
delay(50);
}
}
Discussion