Zenn
🦊

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

2023/11/26に公開
#
電子工作
#
esp32
#
LiDAR
#
compass
#
stepping
tech

地下壕や部屋などの断面形状を測定する携帯型LIDAR（手のひらLIDAR）の第3弾です！前回の手のひらLIDAR ver2で課題だった角度の精度を上げるため、ステッピングモーターでのLIDARコントロールに挑戦しました。結果は上々で、サーボの時よりも段違いに正確な動作ができるようになりました。
　　2024/3/27　RTCを追加してスタンドアロン化しました　→　リンク

目次

  1. はじめに
  2. 全体概要
  3. 使った部品について
  4. レーザー距離計モジュールの制御
  5. ステッピングモーターの制御
  6. OLEDディスプレイの制御
  7. コンパス＆傾きセンサーの制御
  8. 制御用SW、ふた開閉SW
  9. LIDARの制御、動作
  10. フィールドテスト
  11. 制御プログラム

記載されてる技術

  • ステッピングモーターの制御、特に原点復帰（ゼロ点復帰）の機構
  • コンパス（磁気センサー）のキャリブレーション
  • レーザー距離計（LIDAR）ユニットの使い方（怪しい中国製部品の使いこなし）
  • ESP32でのコントロール
  • ESP32をWiFiアクセスポイント＆Webサーバーにして、スマホからアクセス・コントロール

1.初めに

地下壕などを探検していて、穴の大きさや断面形状がどうしても知りたくなるあなたに、モダンでゴージャスな手のひらLIDAR ver1、手のひらLIDAR ver2を作りました。ver1は中国製ロボット掃除機のLIDARモジュール、ver2はレーザー距離計とサーボモーターの組み合わせで実現したんですが、ver1は地下壕にありがちな土壁に対応できずに測定距離に限界があり、ver2はサーボモーターの角度誤差のせいで測定精度がイマイチ・・・。まあ使えるんですが技術者としては不満が残る結果でした。そこで今回は、ver2で使ったサーボモーターをステッピングモーターに変更し、角度の誤差をなくすことにチャレンジしました。サーボモーターは現在角度をエンコーダーでアナログ的に検出して制御するのに対し、ステッピングモーターは1ステップの角度がもう機構・機械的に決まってるので、それはそれは正確なはず。ただし、絶対角度が確定できれば・・・。

2. 全体概要

全体は下のような感じでまとめました。
フタを閉じた状態
フタを開けて測定可能状態にしたとき

壁に向かってレーザー照射
各部の構成
前回の手のひらLIDAR ver2との違いは、

  • スキャン用のサーボモーター　→　ステッピングモーターに変更
  • それに伴って原点復帰動作（後述）が必要になったので、そのためのセンサー追加
  • M5StickCでは制御用のGPIOピンが足りなくなったので、制御コンピューターをフルのGPIOを持つESP32 MH-LIVE Mini Kitに変更（ステッピングモーターの制御にGPIO4本、原点復帰センサーに1本、計5本がモーターのためだけに必要）
  • そうすると傾き検出用の加速度センサーが無くなったので外付け、せっかくなのでコンパス機能も追加
  • LCDディスプレイも無くなったので外付けでOLEDディスプレーを追加
  • スイッチがなくなったので外付け、ついでに3個に増やした
  • フタが閉まった状態でスキャンを開始しないようにフタ開閉検出用のマイクロスイッチを追加

サーボモーターをステッピングモーターに変えたことで、連鎖的に面倒臭いことになっとるやんけ！まあ、分かってたんですけどね・・・。
コントロールはver1、ver2と同じで、ESP32をWiFiアクセスポイントにしてスマホからWiFiで接続、ESP32に立てたWebサーバーを通してHTMLでやります。今回Webページもリニューアルしてちょっとダークカラーにしました。穴の中では白い背景は眩しいのでコウモリ氏の眠りを妨げる可能性ありですからね。
スマホのコントロール画面
回路構成はこんな感じです。

3. 使った部品について

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

ここはver2と同じなので省略。TW10S-UARTのコマンドは英語版も作っておきました。困ってる人多そうなんで。

ステッピングモーター

回転速度は速くなくてもいいので、なるべく細かい角度で動いてくれるやつを選定。ステッピングモーター界では一般的な奴だと思います。ギアで1/64に減速されており、1ステップ11.25/64=0.18°で動作します（2048ステップで360°）。精度としては十分です。ただ、ドライバー基板が大きくてLEDがビカビカ光ってちょっと苦手・・・。
https://www.amazon.co.jp/gp/product/B09M9VX85T
あとそうそう、モーターの軸にレーザー距離計を取り付けるためにカプラーが必要です。こいつがなんか妙に高い！モーターより高いがな！
https://www.amazon.co.jp/gp/product/B0C69HNY5X
後述するように、原点復帰にはフォトインターラプターを用いた位置検出機構（らしきもの）を採用しました。フォトインタラプターはこれを使いました。機構の説明は後程。
https://www.amazon.co.jp/gp/product/B084VP1GXS

制御用コンピューター（ESP32 MH-LIVE Mini Kit）

LIDAR ver1、2で使ってたM5StickCではGPIOのピン数が不足するため、フルGPIOが使えるESP32を使いました。ケース内に入れるものが増えたために小型のMH-LIVE Mini Kitにしました。ブレッドボードも入らないので、電源スイッチ、制御スイッチ、ソケットなどをつけた基板を作って、いい感じ？にまとめました。こればっかりはちゃんと技適マークがついたの買ってくださいね。
https://www.amazon.co.jp/gp/product/B093BX9DL8
基板はこんな感じです。

モバイルバッテリー

ここはver2と同じなので省略。

閉り止め（ステー）

ここもver2と同じなので省略。いや、もう、追加でまとめ買いしたよ。

コンパス＆傾きセンサー

この手のセンサーはいろいろあるんですが、コンパス（3軸磁気センサー）＋3軸加速度センサーが一体化したものを使いました。コンパスで方位を、加速度センサーで本体の傾きを検出します。傾きは、

  • 距離測定の時、三角法で測定物体までの距離、高さを計算する
  • スキャンの時に本体の傾きを測定しておいて、後で傾き補正する

に使います。動きを掴みたいわけではないのでジャイロは不要。ということで、これにしました。
https://ja.aliexpress.com/item/1005005690755747.html

OLEDディスプレイ

アウトドアだと小さい字が見えにくいので、サイズ－値段のコスパが良いの＋I2C制御を探してこれを選びました。で、ライブラリーがなくて結構大変たいがい苦労しました・・・。ネットで使用例が多いのを選んだほうが良いですな。
https://ja.aliexpress.com/item/1005005081809505.html

スイッチ類

電源SWには普通のスライドスイッチ、制御スイッチにはプッシュスイッチを使ってます。フタの開閉を検出するスイッチはこれを使いました。
https://www.amazon.co.jp/gp/product/B09TS4KNB1

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

ここは心臓部ですがver2と同じなので省略！省略！

5. ステッピングモーターの制御

さあ、今回のメインディッシュ、ステッピングモーターの制御です。と言っても、制御自体はステッピングモータードライバーとライブラリにお任せです。サーボモーターとステッピングモーターの違いもネットにいっぱい情報があるので割愛。普通に使ってる限りは非常に正確に角度を刻んでくれます。で、メインはそこではなく、原点復帰（ゼロ点復帰）です

原点復帰の機構

ステッピングモーターは今の角度から＋〇ステップ、－〇ステップという相対的な動きしかできません。なので、ステッピングモーターで絶対角度を制御しようとすると、コントローラー側で角度をちゃんと覚えておいて、「〇度から〇ステップ動かしたから、今は〇度だな」という風に制御しないといけません。しかし、電源を切っちゃったり、モーターを手で回したりしてしまうと今何度か分からなくなってしまいます。なので電源ON時など必要な時に一旦原点（0°位置）まで戻すような機構が必要です。

ここがステッピングモーターの面倒臭いところです。って言うかメーカー何とかしろよ。ゼロ点センサー付きステッピングモーターって売れると思うけどな・・・。まあ、高級品には「レゾルバ」ってのがついてるそうですけど。
で、この原点にどうやって戻すか＝原点に来たことをどうやって検出するか？が考えどころです。やり方としては3つあって、

  1. 当て止め：原点のところにストッパーを取り付けておいて、物理的にそれ以上回らないようにする（モーターには泣いてもらう）
  2. 機械検出：原点のところにマイクロスイッチを取り付けておいて、モーターが回転してきたらそのスイッチが押されるようにする　→　スイッチが押されたことをコントローラー側で検出して「原点に来た！」と判断させる
  3. 光学検出：原点のところにフォトインターラプターを取り付けておいて、モーターが回転してきたら光を遮られるような機構にしておく　→　コントローラー側で検出して「原点に来た！」と判断させる
  4. 磁気検出：原点のところにホールセンサーを、モーター側には磁石を取り付けておき、モーターが回転してきたらホールセンサーと磁石が近づく機構にしておく　→　コントローラー側で検出して「原点に来た！」と判断させる

1はやってみたんですが（やるなよ）、モーターの力が案外強くて、本当にモーターが泣いたので止めました。当て止め、大学の先輩が車を縦列駐車する時に多用してた技です。花田さん元気かな～。2はマイクロスイッチがONになる瞬間がバラツキそうなので不採用。4は磁石がコンパスに干渉するので不採用。ホールセンサーは基本アナログで、「磁束密度〇T以上でON」のような動作なので、磁石の磁力が継時変化したり位置にガタがあるとゼロ点が変わってしまうし。で、今回は3の光学検出を採用しました。光ならもうON/OFFのデジタルなので切れがいい！

光学検出による原点復帰

採用した機構はこんな感じです。ステッピングモーターにカプラーをつけて、アクリル板を取り付けました（アクリル板にレーザー距離計モジュールをマウントする）。そのアクリル板に、100均のウレタンフォームを切って作ったアームを貼り、その先端にプラ板を取り付けました。で、ステッピングモーターが0°付近に来た時にプラ板がちょうどフォトインターラプターを遮るように配置しました（このように位置検出に使う板を専門用語で「ダグ」というそうです）。

動作としては、ステッピングモータが右に回ってきてプラ板がフォトインターラプターの光路を遮ると、インターラプターがONになります。コントローラー側でこれを検出して、「モーターが原点に来た」と判断します。
こんな感じ。

注意点ですが、フォトインターラプターの光は案外強くて白いプラ板を透過してしまうようです。なので、銀色のテープを貼って光が通らないようにしました。

実際の制御

実際の動作はもう少し複雑で、こんなプロトコルで原点復帰させました。

  1. フォトインターラプターがONになるまでステッピングモーターを大きなステップで右回転
  2. モーターを少し左に戻す
  3. 今度はモーターを細かいステップで右回転
  4. フォトインターラプターがONになったところを原点とする
  5. レーザー距離計を取り付けたときに0°の角度になるところまでモーターを回す

1で素早く原点近くまでモーターを動かし、3で高精度で原点を検出します。5は機構的に検出した原点から実際の0°の点まで動かす動作です。
プログラム的には以下のような感じ。
※このプログラムには回しすぎ予防やモーターやセンサーにトラブルがあった場合のタイムアウト処理などが入ってません（後で紹介するフルプログラムには入ってます）。

#include <Stepper.h>

// ステッピングモーター 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°位置までのステップ数
#define MOTOR_SPEED 8                 // ステッピングモーターの回転速度
#define STEP_360 2048                 // 360°のステップ数
volatile float motor_angle = 0;       // モーターの現在角度（ステップ単位）
Stepper Motor(STEP_360, PIN_MOTOR_1, PIN_MOTOR_3, PIN_MOTOR_2, PIN_MOTOR_4);

// ステッピングモーターを指定ステップ進める
void Motor_move(int step)
{
  // 角度方向とステップ符号が逆なので注意
  Motor.step(-step);
  motor_angle = motor_angle + step;
}

// ステッピングモーターの原点復帰
void Motor_zero()
{
  // 原点への荒移動　センサーがON（LOW）になるまで4ステップずつ右回転
  while(digitalRead(PIN_SENSE) == HIGH)
  {
    Motor_move(-4);
    delay(50);
  }
  delay(200);
  // 一旦プラス方向に戻す　センサーがOFF（HIGH）になるまで4ステップずつ左回転
  while(digitalRead(PIN_SENSE) == LOW)
  {
    Motor_move(4);  // 4ステップずつ左回転
    delay(50);
  }
  delay(200);
  // 細かく原点まで戻す　センサーがON（LOW）になるまで1ステップずつ右回転
  while(digitalRead(PIN_SENSE) == HIGH)
  {
    Motor_move(-1);
    delay(50);
  }
  // 0°位置まで移動
  Motor_move(ZERO_POINT);
  motor_angle = 0;
  Serial.println("Zero return OK");
  return;
}

void setup()
{
  Serial.begin(115200);

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

void main()
{
  Motor_zero();
  while(true)
  {
    delay(500);
  }
}

6. OLEDディスプレイの制御

購入したこの一品、なんですが、購入先のWebページによるとチップとして「SSD1116」を使ってるようです。表示解像度も一般的な128x64ドットなので、SSD1116用のライブラリーを使って・・・って、SSD1116って何やねん！そんなライブラリあらへんがな！ネット調べてもそんなチップの情報出てきません。
こうなったら無敵のディスプレイ・ライブラリーU8g2様を使おう。これならものすごい種類のチップに対応してるから、SSD1116にも対応して・・・へんがな！どないすんねん・・・。
他のOLEDで使ってたライブラリーやU8g2の設定を試してみたんですが、どうもうまく動かない・・・。途方に暮れて、U8g2の設定リストをもう一回じっくり見てると、ちゃんとあるやないですか。なんでさっきは見逃したんだろう。と言うことで無事に動くようになりました。
めでたしめでたしで喜んでもう一度見直したら、使った設定は、
   U8G2_SH1106_128X64_NONAME_F_HW_I2C
でした。SSD1116ではなくSH1106用です。どうも煮詰まった挙句、SSD1116のSと11と6だけが頭に残って、夢遊病状態でSH1106の設定を使っちゃったみたいです。まあでもこれで動いてるんで結果オーライ！

7. コンパス＆加速度センサーの制御

難航したOLEDディスプレイに比べ、コンパス＆加速度センサーはすんなりいきました。使ってるチップ「LSM303」に対応したライブラリー
   Adafruit_LSM303_U.h
で問題なく動きます。
コンパスの問題はやっぱりキャリブレーションでしょう。やり方としてはスマホを8の字に回すあれが有名です。装置内で発生するオフセット磁場と地磁気を切り分けるというやつです。ver1では自己流でキャリブレーションしましたが、今回は正規っぽい方法にチャンレジすると同時に、コントロールプログラムにキャリブレーション機能を追加しました。
さて、8の字キャリブレーションですが、やってることは、

  • スマホをいろんな方向を向かせながら磁気センサーの値を読んで、その中の最大値（Mmax）と最小値（Mmin）を記録する
  • Mmaxは偶然センサーが地磁気の方向を向いたとき、Mminは逆方向を向いたときと考えられる
  • なので、余計な磁場（オフセット磁場M0）が無ければ|Mmax|=|Mmin|のはず
  • 違ってるとしたら、それはオフセット磁場の影響
  • なので、計算式
       オフセット磁場：M0 = 0.5 * (Mmax + Mmin)
       地磁気の値：A = 0.5 * (Mmax - Mmin)
        →　補正した磁場値（±1に規格化）= (実測値 - M0)/A

で補正された磁場値が計算できます。
図にするとこんな感じ。

これをX軸、Y軸それぞれにやればOK（Z軸は今回は無視！）。で、atan2(Y磁場, X磁場)で向いてる角度が求まります（センサーを取り付ける方向で±が変わりますが）。
あと、偏角（磁北と実際の北がどれだけ違うかのデータ；本州では-8°ぐらい）がありますので、その分角度を回せば今の方角が割り出せます。
プログラムに組み込んだキャリブレーション機能では、キャリブレーションを終了した時に向いてる方向を北として偏角を設定するようにしました。また、キャリブレーション結果はESP32のEEPROM（フラッシュ）に保存されて、次回起動時に読み込まれます。具体的なやり方は後述。
あと、キャリブレーションの過程で分かったんですが、冷蔵庫から横方向にかなり強い磁場が出ています（うちのだけかもしれませんが）。壁越しに冷蔵庫がある場所でキャリブレーションしてたんでハマりました。皆さん気を付けてください。それにしてもなぜ冷蔵庫から・・・まさかぺ〇ラ・・・あれは磁気じゃなく反重力か・・・。

8. 制御用SW、ふた開閉SW

コントローラーをM5SctickCからESP32に変えたことでスイッチが無くなったので、外付けしました。今回はプッシュスイッチを3つつけて、①原点復帰、②距離連続測定、③測定結果の記憶に割り振りました。これで、単純な距離測定だけならスマホなしで単体でできるようになりました。記憶した測定結果は後でスマホでダウンロードできます。

完成してから思いましたが、もっとスイッチつけときゃよかった。もっと機能盛り込みたかった。まあ、スイッチを単機能にせずにメニューボタンみたいな感じにすればいいんだけど、制御が面倒臭い。
また、フタが閉じてるときにスマホからスキャンの指令を出してしまうと、モーターが動き出して大惨事になります。それを防止するため、フタ開閉センサーを付けました。センサーといっても、フタが閉まるとマイクロスイッチがONになって、コントローラーが「フタ、閉ってます」と認識できるようにしてるだけです。

9. LIDARの制御、動作

本体での制御

前述したように、本体には3つのプッシュスイッチがついており、これで距離測定ができます。SW②を押すとレーザーがONになって距離測定が始まります（もう一回押すとストップ）。傾き（加速度）センサーがあるので、これを使って目標までの水平距離と、目標までの高さを測定できます。原理はこんな感じ（原理っていうほどでもないけど）。

で、その結果をOLEDディスプレイに表示させるようにしました。

これで、市販のレーザー距離計と同じような使い方ができるようになります（屋根までの高さとか測れるので面白いです）。
さらに、測定状態でSW③を押すことで結果をメモリーに記憶できます（100件まで）。メモリーの読み出しはスマホからになります。タイムスタンプ付きで記憶するので、何時ごろ測定したのかも合わせて確認できます。ただし、電源をOFFにしたら消えてしまいますので、電源を切る前にスマホで保存する必要があります。
そこで、「ESP32にはフラッシュがあるのでそこに記憶しておけばいいんじゃ？？」と思ったあなた！その通りっす。でも、ESP32には絶対時間を知るためのRTCがついてないんです！なので、絶対時間でのタイムスタンプがつけられません。後で見て「この結果、いつ測ったんじゃ？？」ということになってしまいます。で、今回どうやってタイムスタンプを打ったかというと、

  • 測定時にESP32の内蔵タイマーmillis()でタイムスタンプをつけておく
  • スマホにデータを保存する際に、スマホから絶対時間を読み込んで、現在のmillis()とタイムスタンプの差から、タイムスタンプ時の絶対時間を割り出す
  • その値を保存する

という、ウルトラマーベラステクノロジー©を使ってます（いや、そこまでする必要あるんか？？）。この辺の苦心が後述のフルプログラムに滲み出てます。このmillis()は電源OFFにするとリセットされてしまいタイムスタンプが無意味になってしまうので「電源を切る前にスマホに保存せよ」ということになります。
絶対時間でタイムスタンプを打ちたかったら電源ON後に内蔵時計を合わせればいいんですが、それが意外に難しい。アウトドアで絶対時間を知る方法は3つ。①絶対時間を覚えておいてくれるRTCを使う、②GPSを使う、③スマホから読み込むです。で、①はESP32にはついてない（実はついてるけど精度が悪い）、②のGPSは起動に数10秒～数分かかるので待ってられないということで却下。③のスマホはまあできるんですが、スマホ接続して時計合わせるんだったら、もうその時にデータ保存したらええやんとなってしまい、今の形式に落ち着きました。そのうち気が向いたらRTCつけるかも。

スマホからの制御

スマホから制御する方法もver2とほとんど同じです。ただし、ユーザーインターフェースは少し変えました。
スマホのコントロール画面
使い方はver1、ver2とほとんど同じで、

  • LIDAR起動
  • スマホのWiFi接続先をLIDARにする
       SSID:TENOHIRA_LIDAR3   PASSWORD:なし
  • スマホからWebサーバーにアクセスする
       WebサーバーのIPアドレス：192.168.11.1
  • コントロール画面になる
  • コントロール画面から、距離測定、スキャン（LIDAR動作）、データ保存などの操作を行う

という感じで使います。
コントロール画面の上方の測定結果表示エリアと、下方のスキャン結果表示エリアはver2から変更ありません。中央部のコントロールエリアのみ整理＆変更してます。
変更点は、

  • 距離測定（連続測定）にメモリーボタンの追加　→　現在の測定値を記憶できます
  • スキャンのパラメーターをセレクトボックスで選択できるようにしました
  • LIDARの角度変更のところに、原点復帰（ZERO）ボタンを追加しました
  • コンパスのキャリブレーションボタンを追加しました

です。
あと、内部的な処理もこっそり変わっています。大きなものとしては、HTML通信待ち受けをマルチタスクにして接続の安定度をUPしています。

コンパスのキャリブレーション

前述したように、今回は8の字キャリブレーションの機能を組み込んでます。やり方は、

  • 磁気の影響の無さそうなところへ行く（冷蔵庫の近くはダメ！絶対！）
  • スマホから「CALIB」ボタンを押す
  • キャリブレーションモードに入る（OLEDにX、Y、Z方向のMax、Min、角度AZが表示される）
  • LIDARを水平に8の字に何度も回す（アウトドアの場合は自分ごとぐるぐる回る）
  • X、YのMaxが30～50、Minが-30～-50ぐらいになったら大体OKと思われる
  • 北、東、南、西を向いてみて、角度AZが大体0°、90°、180°、-90°になるか確認する
  • LIDARを真北に向ける　→　ここ注意！これで偏角を合わせます
  • そのままスマホの「STOP」ボタンを押す

これでキャリブレーション完了です。距離測定モードにして、ちゃんと方角があってるか確認してください。キャリブレーション結果はEEPROMに保存され、次回以降は起動時にその値が読み込まれます。

保存されるデータファイル

スマホの「SAVE DATA」ボタンで保存されるデータの内容も書いておきます。距離測定データの保存機能を付けたので、そのデータが最後尾についてます。

LIDAR3 data start
Control program v1.4
Scan data
2023/11/25 15:55:41                    // 日時
Tilt(x y z)=-0.039,1.765,10.003        // 加速度センサーの測定値（傾き）
Compass(x y z)=1.182,-39.545,-53.878   // コンパスの測定値（補正なし）
Azimuth=74.4                           // 方位角（補正あり）
Data number=273                        // スキャンデータの点数
angle,distance,x,z
-5.6250,101.1,100.6,-9.9               // スキャンデータ（角度、距離、X位置、Z位置）
-4.9219,98.9,98.5,-8.5
・・・・・
185.6250,-1.0,1.0,0.1

Memory data                            // 距離測定保存データ
number,day,time,distance,angle,azimuth,mag x,y,z,tilt x,y,z
1,2023/11/12,16:47:56,740.7,0.000,-7.74-25.818-2.364-40.408-0.588-0.03910.238
2,2023/11/12,16:49:0,-1.0,0.000,7.68-26.273-10.636-40.102-0.745-0.11810.238
・・・・・

7. フィールドテスト

室内テスト

取りあえず我が家の部屋で測定してみました。

良かった！我が部屋、ちゃんと真四角だった！今回サーボモーターからステッピングモーターに変えたことで角度が正確になり、測定精度が爆上がりです！（ver2のいびつな測定結果と比較してみて下され）。サーボの角度誤差で散々苦労したのがバカバカしいったらありゃしない。もう時代はステッピングモーターですな。これで原点復帰だけもっと簡単になれば言うことないのに・・・。
あと、床が斜めになってる場合です。地下壕では床面凸凹グアノ注意なことが多く、LIDARを水平に置くのが難しいことがあります。この傾きは傾きセンサー（加速度センサー）の測定値から補正することができます。わざとLIDARを斜めにして測定すると、こんな感じのデータになります。

で、データには傾きセンサーの測定値が入ってるので、それを使って補正をかけることができます。結果は下のような感じで、ちゃんと傾きが補正されて、垂直面を上にしたデータが得られました。

あとは実際の地下壕でどんだけいいデータがとれるかやね。

フィールドテスト

近日公開

8. 制御プログラム

制御プログラムです。「モーターを動かしてレーザー距離計で距離を測る」という基本的なところはver2から変わってませんが、制御が増えた＆頭がよくなったので大幅に改定してます。相変わらず壮大なものになっており、やっぱり詳細な説明は面倒くさいので、適当に読み解いてください。「ここはもっとこうせんかい！！」というアドバイスいただけますと幸いです。

プログラム構成

  • ディスプレイ
    Display(int line, String word)：OLEDディスプレイに文字列を表示
  • ステッピングモーター関係
    Motor_stop()：ステッピングモーターを待機状態に（電力削減）
    Motor_move(int step)：ステッピングモーターを指定ステップ進める
    Motor_zero()：ステッピングモーターの原点復帰
  • コンパス、傾きセンサー
    Compass()：コンパス＆傾きセンサー測定
    Compass_calib()：コンパスのキャリブレーション
  • レーザー距離計
    LIDAR_measurement()：レーザー距離計測定
  • LIDAR動作
    LIDAR(void *pvParameters)：マルチタスクで常時稼働しており、Main側でlidar_stateの値を変更すると対応する測定を開始します。
    angleSearch(float search_angle)：指定角度に最も近いデータ番号を検索する
  • Webサーバー、コントロール関係
    controlPage()：コントールページをスマホ側に送信する
    dataTransfer()：測定データファイルをスマホ側に送信する
    handleAccess()：スマホからのアクセス対応
    handleNotFound()：スマホからのリクエストにエラーがある場合
    waitingClient(void *pvParameters)：Webサーバーのリクエスト待ち（マルチタスクで常時稼働）
  • スイッチが押された時の処理（割り込み）
    IRAM_ATTR SW1_pushed()：スイッチが押された場合の割り込み処理
    IRAM_ATTR SW2_pushed()：スイッチが押された場合の割り込み処理
    IRAM_ATTR SW3_pushed()：スイッチが押された場合の割り込み処理
  • メインルーチン
    void setup()：初期化
    void loop()：メインルーチン

プログラム

/////////////////////////////////////////////////////////////////////////
//                                                                     //
//  LIDAR3 CONTROL Program ver1.4   2023/11/25  Inu-Kistune            //
//     ESP32 MH-LIVE Mini Kit                                          //
//     Lidar TW10S-UART                                                //
//     Stepping motor 28BYJ-48 + driver ULN2003                        //
//     OLED 128x64(SH1106)                                             //
//     Compass&Acc LSM303                                              //
//                                                                     //
/////////////////////////////////////////////////////////////////////////

#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 <EEPROM.h>

#define PIN_I2C_SDA 21
#define PIN_I2C_SCL 22
#define PIN_LED 2

///////////////////////////////////////////////
// 初期設定
///////////////////////////////////////////////

// グローバル変数
#define MAX_DATA 520        // 最大測定点
volatile int LIDAR_ndata;                   // データ点数
volatile float LIDAR_currentdistance;       // 現在の測定距離
volatile float LIDAR_angle[MAX_DATA];       // スキャン角度
volatile float LIDAR_distance[MAX_DATA];    // スキャン距離
float LIDAR_azimuth;                        // スキャン時の方角
float LIDAR_mag[3];                         // 測定時の方角
float LIDAR_tilt[3];                        // スキャン時の加速度センサーデータ
#define MAX_MEMORY 100                      // 最大メモリー点
int MEMORY_ndata = 0;
float MEMORY_angle[MAX_MEMORY];             // 測定角度
float MEMORY_distance[MAX_MEMORY];          // 測定距離
float MEMORY_azimuth[MAX_MEMORY];           // 測定時の方角
float MEMORY_mag[3][MAX_MEMORY];            // 測定時の方角
float MEMORY_tilt[3][MAX_MEMORY];           // 測定時の加速度センサーデータ
int MEMORY_millis[MAX_MEMORY];              // 測定時の時刻（millis）

// WiFi & Web server
WebServer server(80);
const char* APssid = "TENOHIRA_LIDAR3";       // AP SSID
const char* APpassword = "";                  // AP password
IPAddress ipadd(192,168,11,1);                // Fixed IP Address
IPAddress subnet(255,255,255,0);              // Subnet mask
int autorequest = false;                      // TrueならWebページが自動読み込みモードになる

// Display SSD1116 128x64
U8G2_SH1106_128X64_NONAME_F_HW_I2C display(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);

// Accerometer & Compass LSM303
Adafruit_LSM303_Accel_Unified LSM303acc = Adafruit_LSM303_Accel_Unified(54321);
Adafruit_LSM303_Mag_Unified LSM303mag = Adafruit_LSM303_Mag_Unified(12345);
struct COMPASS_CALIB
{
  int saved;                 // キャリブレーション済みorなし
  float AX, AY, AZ;          // コンパス　振幅値
  float M0X, M0Y, M0Z;       // コンパス　オフセット値
  float declination;         // コンパス　偏角
};
COMPASS_CALIB calib_data;
int compass_ok;
float accX, accY, accZ;
float magX, magY, magZ;
float elevation, azimuth;

// LIDAR TW10S
#define PIN_LIDAR_Rx 17
#define PIN_LIDAR_Tx 16
#define PIN_LIDAR_Active 26
#define LIDAR_TIMEOUT 1000                  // レーザー距離計の応答待ちタイムアウト時間
#define LIDAR_WAIT 800                      // サーボを動かしてから測定までの待ち時間（msec）
#define DISTANCE_OFFSET 4                   // レーザー距離計のオフセット長（cm）；レンズ～回転中心までの長さ
// LIDAR変数
#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_COMPASS_CALIB 4               // LIDARの状態定数（4=コンパスのキャリブレーション）
volatile int lidar_state = LIDAR_WAITING;   // LIDARの状態
int ang_start, ang_end, ang_step;           // Scanパラメーター
int scanlevel = 3;                          // Scan種類
int lidar_ok;
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};       // 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

// ステッピングモーター 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;       // モーターの現在角度（ステップ単位）
int motor_ok;
Stepper Motor(STEP_360, PIN_MOTOR_1, PIN_MOTOR_3, PIN_MOTOR_2, PIN_MOTOR_4);

// Switch
#define PIN_SW1 27
#define PIN_SW2 25
#define PIN_SW3 32
int SW1_time = -100;                  // チャタリング防止のタイマー
int SW2_time = -100;
int SW3_time = -100;
int SW1_processing = false;           // スイッチON/OFF
int SW2_processing = false;
int SW3_processing = false;
#define PIN_COVER 15

///////////////////////////////////////////////
// 関数
///////////////////////////////////////////////

// OLED表示
void Display(int line, String word)
{
  display.setCursor(0, 13 * line - 1);
  if(line == 1)
  {
    display.print(word + "                ");
  }
  else
  {
    display.print(" " + word + "               ");
  }
  display.sendBuffer();
}

// モーターを停止
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)
{
  // 角度方向とステップ符号が逆なので注意
  int 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()
{
  int timer = millis();
  int flag = true;

  lidar_state = LIDAR_READY;
  motor_angle = 0;

  // 原点への荒移動
  Serial.println("Finding origin");
  Display(1, "Finding origin");
  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(1, "Origin found");
    // 一旦プラス方向に戻す
    timer = millis();
    while(digitalRead(PIN_SENSE) == LOW)
    {
      if(millis() > timer + 5000)   // タイムアウト5秒
      {
        flag = false;
        break;
      }
      motor_angle = 0;
      Motor_move(4);
      delay(50);
    }
    delay(200);
  }
  if(flag == true)
  {
    // 細かく原点まで戻す
    timer = millis();
    while(digitalRead(PIN_SENSE) == HIGH)
    {
      if(millis() > timer + 5000)
      {
        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(1, "Motor at 0deg");
  }
  else
  {
    Serial.println("Zero return error");
    Display(1, "Zero return error");
  }
  Motor_stop();
  delay(1000);
  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()
{
  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;

  Display(1, "Compass Calib.");

  while(lidar_state == LIDAR_COMPASS_CALIB)
  {
    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(2, "X " + String(max_magX, 2) + " " + String(min_magX, 2));
    Display(3, "Y " + String(max_magY, 2) + " " + String(min_magY, 2));
    Display(4, "Z " + String(max_magZ, 2) + " " + String(min_magZ, 2));
    Display(5, "AZ " + String(azimuth, 1));
    delay(50);
  }
  // キャリブレーション値設定
  calib_data.saved = 77;
  calib_data.AX = cAX;
  calib_data.AY = cAY;
  calib_data.AZ = cAZ;
  calib_data.M0X = cM0X;
  calib_data.M0Y = cM0Y;
  calib_data.M0Z = cM0Z;
  calib_data.declination = -(azimuth - cDeclination);    // キャリブレーション停止時の方向を北とする

  // EEPROMへ書き込み
  EEPROM.put<COMPASS_CALIB>(0, calib_data);
  EEPROM.commit();

  return true;
}

// LIDAR測定
float LIDAR_measurement()
{
  int count1;
  int timeout = true;
  float distance;
  char buf;
  char data[20];

  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;    // 測定値エラー
    }
  }
  return distance;
}

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

/////////////////////////////////////////////////////////////////////////
// LIDAR measurement
//    マルチタスクで常時稼働しています
//    Main側でlidar_stateをLIDAR_MEASURINGに変えると連続測定開始
//    LIDAR_SCANNINGにするとスキャン開始
/////////////////////////////////////////////////////////////////////////
void LIDAR(void *pvParameters)
{
  int ret, num;
  int current_angle;
  float current_distance;
  float original_angle = motor_angle;

  while(true)
  {
    // 連続測定開始
    if(lidar_state == LIDAR_MEASURING) 
    {
      digitalWrite(PIN_LED, HIGH);
      Display(1, "LIDAR MEASURING");
      UART_LIDAR.write(lidar_countinuous, sizeof(lidar_countinuous));
      while(lidar_state == LIDAR_MEASURING)
      {
        LIDAR_currentdistance = LIDAR_measurement();
        ret = Compass();
        Serial.print("Acc X/Y/Z/EL=" + String(accX, 2) + "/" + String(accY, 2) + "/" + String(accZ, 2) + "/" + String(elevation,1) + "  ");
        Serial.println("Mag X/Y/Z/AZ=" + String(magX, 1) + "/" + String(magY, 1) + "/" + String(magZ, 1) + "/" + String(azimuth,1));
        Serial.println("DISTANCE=" + String(LIDAR_currentdistance, 1) + "cm");
        Display(2, "DIST=" + String(LIDAR_currentdistance,1) + "cm");
        Display(3, "DT/HT=" + String(LIDAR_currentdistance * cos(elevation / 180 * PI), 1) + "/" + String(LIDAR_currentdistance * sin(elevation / 180 * PI), 1));
        Display(4, "EL=" + String(elevation,0) + char(176) + "/AZ=" + String(azimuth,0) + char(176));
        Display(5, "Memo=" + String(MEMORY_ndata));
        delay(300);
      }
      UART_LIDAR.write(lidar_stop, sizeof(lidar_stop));
      digitalWrite(PIN_LED, LOW);
      Display(1, "LIDAR WAITING");
    }
    // スキャン開始
    if(lidar_state == LIDAR_SCANNING)
    {
      if(ang_start < MIN_ANGLE || ang_start > MAX_ANGLE || ang_end < MIN_ANGLE || ang_end > MAX_ANGLE)
      {
        ret = false;
      }
      else
      {
        Display(1, "LIDAR SCANNING");
        digitalWrite(PIN_LED, HIGH);
        // コンパスデータ取得
        ret = Compass();
        LIDAR_azimuth = azimuth;
        LIDAR_mag[0] = magX;
        LIDAR_mag[1] = magY;
        LIDAR_mag[2] = magZ;
        LIDAR_tilt[0] = accX;
        LIDAR_tilt[1] = accY;
        LIDAR_tilt[2] = accZ;
        // LIDARアクティブ
        UART_LIDAR.write(lidar_countinuous, sizeof(lidar_countinuous));
        current_angle = ang_start;
        num = 0;
        // スキャン
        while(current_angle <= ang_end && lidar_state == LIDAR_SCANNING)
        {
          Motor_move(current_angle - motor_angle);
          delay(LIDAR_WAIT);   // LIDARの測定待ち
          LIDAR_angle[num] = current_angle * ANGLE_PER_STEP;
          LIDAR_currentdistance = LIDAR_measurement();
          LIDAR_distance[num] = LIDAR_currentdistance;
          Serial.println(String(current_angle * ANGLE_PER_STEP, 1) + "," + String(LIDAR_currentdistance, 1));
          Display(2, "DISTANCE=" + String(LIDAR_currentdistance,1) + "cm");
          Display(3, "ANGLE=" + String(current_angle * ANGLE_PER_STEP,1) + char(176));
          current_angle = current_angle + ang_step;
          num++;
          LIDAR_ndata = num;
        }
        if(lidar_state == LIDAR_SCANNING)   // 正常終了
        {
          Serial.println("Data num = " + String(LIDAR_ndata));
          Serial.println("Tilt = " + String(LIDAR_tilt[0], 1) + "/" + String(LIDAR_tilt[1], 1) + "/" + String(LIDAR_tilt[2], 1));
          Serial.println("Azimuth = " + String(LIDAR_azimuth, 1));
        }
        UART_LIDAR.write(lidar_stop, sizeof(lidar_stop));
        Motor_move(original_angle - motor_angle);
        Motor_stop();
        lidar_state = LIDAR_READY;
        Display(1, "LIDAR WAITING");
        digitalWrite(PIN_LED, LOW);
      }
    }
    // スキャン開始
    if(lidar_state == LIDAR_COMPASS_CALIB)
    {
      Compass_calib();
    }    
    delay(100);
  }
}

/////////////////////////////////////////////////////////////////////////
// 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>LIDAR3</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: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) + "&deg;</td>";
  html += "<td>DISTANCE</td>";
  html += "<td>" + String(LIDAR_currentdistance, 1) + "cm</td></tr>";
  html += "<tr align=\"left\"><td></td><td>0&deg;</td><td></td><td>";
  html += String(LIDAR_distance[angleSearch(0)], 1) + "cm</td></tr>";
  html += "<tr align=\"left\"><td></td><td>90&deg;</td><td></td><td>";
  html += String(LIDAR_distance[angleSearch(90)], 1) + "cm</td></tr>";
  html += "<tr align=\"left\"><td></td><td>180&deg;</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>MEAS.</td>";
  html += "<td><div style =\"display:inline-flex\">";
  html += "<form><button style=\"font-size:100%\" type=\"submit\" name=\"scan\" value=\"scan0\">START</button></form>";
  html += "&emsp;&emsp;<form><button style=\"font-size:100%\" type=\"submit\" name=\"command\" value=\"memory\">MEMORY</button></form>";
  html += "&emsp;N=" + String(MEMORY_ndata) + "</div></td></tr>";
  html += "<tr align=\"left\"><td>SCAN</td>";
  html += "<td><form> <input style =\"font-size:100%\" type=\"submit\" value=\"START\">";
  html += "&emsp;<select style =\"font-size:100%\" name=\"scan\">";
  html += "<option value=\"scan1\"";
  if(scanlevel == 1) html += " selected";
  html += ">11.3&deg;/step</option>";
  html += "<option value=\"scan2\"";
  if(scanlevel == 2) html += " selected";
  html += ">5.6&deg;/step</option>";
  html += "<option value=\"scan3\"";
  if(scanlevel == 3) html += " selected";
  html += ">2.8&deg;/step</option>";
  html += "<option value=\"scan4\"";
  if(scanlevel == 4) html += " selected";
  html += ">0.7&deg;/step</option>";
  html += "<option value=\"scan5\"";
  if(scanlevel == 5) html += " selected";
  html += "> 0.35&deg;/step</option></select></form></td></tr>";
  // ステッピングモーターの角度設定
  html += "<tr align=\"left\"><td>ANGLE</td>";
  html += "<td><div style=\"display:inline-flex\">";
  html += "<form><input style=\"font-size:100%;\" type=\"submit\" value=\"SET\">";
  html += "&emsp;<input style=\"font-size:100%;width:3em\" type=\"number\" min=\"-5\" max=\"185\" name=\"angle\" value=\"";
  html += String(motor_angle*ANGLE_PER_STEP,1) + "\">&deg;</form>";
  html += "&emsp;&emsp;<form style=\"display: inline\">&emsp;<button style=\"font-size:100%\" type=\"submit\" name=\"angle\" value=\"zero\">ZERO</button></form></div></td></tr>";
  // STOP & Save & Calibration
  html += "<tr align=\"center\"><td colspan=\"2\"><div style=\"display:inline-flex\">";
  html += "<form><button style =\"font-size:100%\" type=\"submit\" name=\"command\" value=\"stop\">STOP</button></form>";
  html += "&emsp;&emsp;<input type=\"button\" style=\"font-size:100%\" value=\"SAVE DATA\" onclick=\"saveData()\">";
  html += "&emsp;&emsp;<form><button style =\"font-size:100%\" type=\"submit\" name=\"command\" value=\"calib\">CALIB</button></form></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 dataTransfer()
{
  int count1;
  String datafile;
  String datetime;
  int current_year, current_month, current_day;
  int current_hour, current_minute, current_second;
  int measure_hour, measure_minute, measure_second;
  int current_millis, measure_millis;

  // スマホから送られてきた日付データを取得
  datetime = server.arg("datetime");
  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();
  current_millis = millis();

  Serial.println("Send data to client");
  Display(1, "SAVE DATA");

  // 測定データファイルを作成
  // ヘッダー
  datafile = "LIDAR3 data start\n";
  datafile += "Control program v1.4\n";
  datafile += "Scan data\n";
  datafile += String(current_year) + "/" + String(current_month) + "/" + String(current_day) + " ";
  datafile += String(current_hour) + ":" + String(current_minute) + ":" + String(current_second) + "\n";
  datafile += "Tilt(x y z)=" + String(LIDAR_tilt[0],3) + "," + String(LIDAR_tilt[1],3) + "," + String(LIDAR_tilt[2],3) + "\n";
  datafile += "Compass(x y z)=" + String(LIDAR_mag[0],3) + "," + String(LIDAR_mag[1],3) + "," + String(LIDAR_mag[2],3) + "\n";
  datafile += "Azimuth=" + String(LIDAR_azimuth,1) + "\n";
  datafile += "Data number=" + String(LIDAR_ndata) + "\n";
  datafile += "angle,distance,x,z\n";
  // スキャンデータ
  for (count1 = 0; count1 < LIDAR_ndata; count1++)
  {
    datafile += String(LIDAR_angle[count1],4) + ",";
    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";
  }
  // 記憶している定点データ
  datafile += "\n\n";
  datafile += "Memory data\n";
  datafile += "number,day,time,distance,angle,azimuth,mag x,y,z,tilt x,y,z\n";
  for (count1 = 0; count1 < MEMORY_ndata; count1++)
  {
    // スマホから送られてきたデータで内部時間（millis）を実時間に変換（日付変更はサポートしてない）
    measure_millis = 1000 * (current_second + 60 * (current_minute + 60 * current_hour)) - (current_millis - MEMORY_millis[count1]);
    measure_hour = int(measure_millis/(60*60*1000));
    measure_minute = int((measure_millis - measure_hour * 60 * 60 * 1000) / (60 * 1000));
    measure_second = int((measure_millis - (measure_hour * 60 + measure_minute) * 60 * 1000)/1000);
    // 測定データファイルを作成
    datafile += String(count1 + 1) + "," + String(current_year) + "/" + String(current_month) + "/" + String(current_day) + ",";
    datafile += String(measure_hour) + ":" + String(measure_minute) + ":" + String(measure_second)  + ",";
    datafile += String(MEMORY_distance[count1],1) + ",";
    datafile += String(MEMORY_angle[count1], 3) + "," + String(MEMORY_azimuth[count1]) + ",";
    datafile += String(MEMORY_mag[0][count1], 3) + "," + String(MEMORY_mag[1][count1], 3) + "," + String(MEMORY_mag[2][count1], 3) + ",";
    datafile += String(MEMORY_tilt[0][count1], 3) + "," + String(MEMORY_tilt[1][count1], 3) + "," + String(MEMORY_tilt[2][count1], 3);
    datafile += "\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(digitalRead(PIN_COVER) == HIGH)
  {
    if (server.hasArg("scan"))
    {
      if (server.arg("scan") == "scan0")        // Scan0（角度を変えずに連続測定）
      {
        lidar_state = LIDAR_MEASURING;
      }
      if (server.arg("scan") == "scan1")        // Scan1（0～180°まで11.5°ステップで測定）
      {
        scanlevel = 1;
        ang_start = 0;
        ang_end = 1024;
        ang_step = 64;
        lidar_state = LIDAR_SCANNING;
      } 
      if (server.arg("scan") == "scan2")        // Scan2（-5.6～185.6°まで5.6°ステップで測定）
      {
        scanlevel = 2;
        ang_start = -32;
        ang_end = 1056;
        ang_step = 32;
        lidar_state = LIDAR_SCANNING;
      } 
      if (server.arg("scan") == "scan3")        // Scan3（-5.6～185.6°まで2.8°ステップで測定）
      {
        scanlevel = 3;
        ang_start = -32;
        ang_end = 1056;
        ang_step = 16;
        lidar_state = LIDAR_SCANNING;
      } 
      if (server.arg("scan") == "scan4")        // Scan4（-5.6～185.6°まで0.7°ステップで測定）
      {
        scanlevel = 4;
        ang_start = -32;
        ang_end = 1056;
        ang_step = 4;
        lidar_state = LIDAR_SCANNING;
      } 
      if (server.arg("scan") == "scan5")        // Scan5（-5.6～185.6°まで0.35°ステップで測定）
      {
        scanlevel = 5;
        ang_start = -32;
        ang_end = 1056;
        ang_step = 2;
        lidar_state = LIDAR_SCANNING;
      } 
      autorequest = true;
    }
  }
  // コマンドの場合
  if (server.hasArg("command")) 
  {
    if (server.arg("command") == "memory")    // 現在の測定データをメモリーに記憶
    {
      if(MEMORY_ndata < MAX_MEMORY)
      {
        MEMORY_angle[MEMORY_ndata] = motor_angle*ANGLE_PER_STEP;
        MEMORY_distance[MEMORY_ndata] = LIDAR_currentdistance;
        MEMORY_azimuth[MEMORY_ndata] = azimuth;
        MEMORY_mag[0][MEMORY_ndata] = magX;
        MEMORY_mag[1][MEMORY_ndata] = magY;
        MEMORY_mag[2][MEMORY_ndata] = magZ;
        MEMORY_tilt[0][MEMORY_ndata] = accX;
        MEMORY_tilt[1][MEMORY_ndata] = accY;
        MEMORY_tilt[2][MEMORY_ndata] = accZ;
        MEMORY_millis[MEMORY_ndata] = millis(); 
        MEMORY_ndata += 1;
      }
    }
    if (server.arg("command") == "calib")    // コンパスのキャリブレーションモード
    {
      lidar_state = LIDAR_COMPASS_CALIB;
    }
    if (server.arg("command") == "stop")      // STOPコマンド
    {
      lidar_state = LIDAR_READY;
    } 
  }
  if(lidar_state == LIDAR_READY)
  {
    autorequest = false;
  }
  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();
      }
    }
  }
  controlPage();
}

// スマホからのリクエストにエラーがある場合
void handleNotFound()               
{
  server.send(404, "text/plain", "Not Found");
}

// Webサーバーのリクエスト待ち（マルチタスク）
void waitingClient(void *pvParameters)
{
  while(true)
  {
    server.handleClient();
    delay(10);
  }
}

/////////////////////////////////////////////////////////////////////////
// スイッチが押された場合の割り込み処理
/////////////////////////////////////////////////////////////////////////
void IRAM_ATTR SW1_pushed()
{
  if(digitalRead(PIN_COVER) == HIGH)
  {
    if(SW1_processing == false)
    {
      SW1_processing = true;
    }
  }
}

void IRAM_ATTR SW2_pushed()
{
  if(digitalRead(PIN_COVER) == HIGH)
  {
    if(millis() > SW2_time + 2000)
    {
      if(SW2_processing == false)
      {
        SW2_processing = true;
      }
      SW2_time = millis();
    }
  }
}

void IRAM_ATTR SW3_pushed()
{
  if(digitalRead(PIN_COVER) == HIGH)
  {
    if(millis() > SW3_time + 2000)
    {
      if(SW3_processing == false)
      {
        SW3_processing = true;
      }
      SW3_time = millis();
    }
  }
}

/////////////////////////////////////////////////////////////////////////
// Setup
/////////////////////////////////////////////////////////////////////////
void setup()
{
  int timer;

  lidar_ok = false;
  compass_ok = false;
  motor_ok = false;

  Serial.begin(115200);
  Serial.println("Initializing");

  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_LED, LOW);
  
  Wire.begin(PIN_I2C_SDA,PIN_I2C_SCL);
  Serial.println("I2C OK");

  // 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(1, "Display OK");

  // 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(1, "WiFi setup");
  delay(500);

  // Web server
  server.on("/", handleAccess);
  server.on("/Lidardata", dataTransfer);
  server.onNotFound(handleNotFound);
  server.begin();
  Serial.println("Web server begin");
  Display(1, "Web server setpu");
  delay(200);
  xTaskCreateUniversal(waitingClient, "waitingClient", 8192, NULL, 5, NULL, PRO_CPU_NUM );   // Web待ち受けタスク起動

  // LSM303 Accmeter & Compass
  if (LSM303acc.begin() && LSM303mag.begin())
  {
    Serial.println("Compass OK");
    Display(1, "Compass OK");
    compass_ok = true;
    // EEPROMからキャリブレーション設定読み込み
    EEPROM.begin(200);    // 200byte確保
    EEPROM.get<COMPASS_CALIB>(0, calib_data);
    if (calib_data.saved != 77)  // 保存データが無い場合はデフォルト値を使用する
    {
      calib_data.AX = 50;
      calib_data.AY = 50;
      calib_data.AZ = 50;
      calib_data.M0X = 0;
      calib_data.M0Y = 0;
      calib_data.M0Z = 0;
      calib_data.declination = -8.0;
    }
  }
  else
  {
    Serial.println("Compass NG");
    Display(1, "Compass NG");
    compass_ok = false;
    delay(2000);
  }
  delay(100);

  // 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(1, "Motor OK");
    motor_ok = true;
  }
  else
  {
    Serial.println("Motor NG");
    Display(1, "Motor NG");
    motor_ok = false;
    delay(2000);
  }
  delay(100);

  // Switch
  pinMode(PIN_SW1, INPUT_PULLUP);
  pinMode(PIN_SW2, INPUT_PULLUP);
  pinMode(PIN_SW3, INPUT_PULLUP);
  attachInterrupt(PIN_SW1, SW1_pushed, FALLING);
	attachInterrupt(PIN_SW2, SW2_pushed, FALLING);
	attachInterrupt(PIN_SW3, SW3_pushed, FALLING);
  pinMode(PIN_COVER, INPUT_PULLUP);
  Serial.println("SW OK");
  Display(1, "SW OK");
  delay(100);

  // 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(1, "LIDAR Initializing");
  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(1, "LIDAR OK");
    lidar_state = LIDAR_READY;
    xTaskCreateUniversal(LIDAR, "LIDAR", 8192, NULL, 5, NULL, APP_CPU_NUM);   // LIDAR測定タスクを開始
  }
  else
  {
    Serial.println("LIDAR NG");
    Display(1, "LIDAR NG");
    lidar_state = LIDAR_READY;
    delay(2000);
  }
  delay(200);
  Serial.println("Initialize OK");
  Display(1, "Initialize OK");
  delay(200);
  Display(1, "LIDAR V3 1.4");
}

/////////////////////////////////////////////////////////////////////////
// Main loop
/////////////////////////////////////////////////////////////////////////
void loop()
{
  while(true)
  {
    // スイッチプッシュ時の動作
    // SW1　モーターゼロ点
    if(SW1_processing == true)
    {
      Motor_zero();
      SW1_processing = false;
    }
    // SW2　連続測定ON/OFF
    if(SW2_processing == true)
    {
      SW2_processing = false;
      if(lidar_state == LIDAR_READY)
      {
        lidar_state = LIDAR_MEASURING;
      }
      else if(lidar_state == LIDAR_MEASURING)
      {
        lidar_state = LIDAR_READY;
      }
    }
    // SW3　連続測定点メモリー
    if(SW3_processing == true)
    {
      SW3_processing = false;
      if(MEMORY_ndata < MAX_MEMORY)
      {
        MEMORY_angle[MEMORY_ndata] = motor_angle*ANGLE_PER_STEP;
        MEMORY_distance[MEMORY_ndata] = LIDAR_currentdistance;
        MEMORY_azimuth[MEMORY_ndata] = azimuth;
        MEMORY_mag[0][MEMORY_ndata] = magX;
        MEMORY_mag[1][MEMORY_ndata] = magY;
        MEMORY_mag[2][MEMORY_ndata] = magZ;
        MEMORY_tilt[0][MEMORY_ndata] = accX;
        MEMORY_tilt[1][MEMORY_ndata] = accY;
        MEMORY_tilt[2][MEMORY_ndata] = accZ;
        MEMORY_millis[MEMORY_ndata] = millis(); 
        MEMORY_ndata += 1;
      }
    }
    if(digitalRead(PIN_COVER) == LOW)
    {
        lidar_state = LIDAR_READY;
    }
    delay(50);
  }
}

おしまい（2023/12/25　イヌキツネ）

Discussion