正面までの距離を読む ―
LiDAR をつなぐ

LiDAR をつなぐのは、線4本とプログラム1本です。ここでは正面の1方向だけ、距離を読むところまでやります。

LiDAR が何をする部品なのかは、まわり全部の距離を知る にあります。

つなぐ ― 線は4本

Tx と Rx はたすき掛けです。LiDAR の Tx（出る線）が Pico の受け口へ、Pico の出る線が LiDAR の Rx へ。同じ名前どうしをつなぐと動きません。

電源でつまずく ― 起動の瞬間に 1A

ここがいちばんの関門です。LiDAR はモーターで回るので、電気をよく食べます。

Pico の VBUS（40番ピン）は、USB から来た 5V がそのまま出ています。机の上で試すだけなら、ここから取れます。ただしパソコンの USB ポートによっては足りません。

ロボットに載せるときは、別の 5V から取ります。バッテリーから 2A 以上出せる DC-DC を通し、GND だけ Pico と共通にします。モーターと同じ電源から取ると、モーターが動いた瞬間に LiDAR が止まります。

送られてくるデータの形

電源を入れただけでは、LiDAR は回りません。「始めてください」と伝える必要があります。送るのは2バイトです。

開始を送ると、あとは測った点が流れ続けます。速さは 230400bps。ふつうのシリアル通信（115200bps）の倍です。

流れてくるのは、こういう固まりです。

角度は2つしか入っていません。40点あっても、入っているのは最初と最後だけ。あいだの点は、2つの角度を等間隔で割って求めます。

距離の取り出し方には、ひとくせあります。

角度も距離も、下のビットが距離や角度そのものではありません。ずらさずにそのまま数として読むと、距離は4倍の値が出ます。ここはつまずきやすいところです。

プログラム ― 正面の距離を読む

そのまま書き込めます。正面（0度）にいちばん近い点の距離と強度を、0.5秒ごとに出します。

#include <Arduino.h>

// ------------------------------------------------------------ 設定
const int PIN_LIDAR_TX = 0;  // GP0 → LiDAR の Rx
const int PIN_LIDAR_RX = 1;  // GP1 ← LiDAR の Tx

// LiDAR の0度がロボット正面から何度ずれているか（時計回りが正）
const float MOUNT_OFFSET_DEG = 0.0f;

const float FRONT_DEG = 0.0f;   // 測る方向（0 = 正面）
const float WINDOW_DEG = 2.0f;  // その ±2度以内の点だけ使う
const uint16_t MIN_MM = 50;     // データシートの最短測距。これより近い値は捨てる

// ------------------------------------------------------------ 結果
uint16_t frontMm = 0;        // 直前の1周で測れた距離。0 = 測れなかった
uint8_t frontIntensity = 0;  // そのときの強度
float frontAngle = 0;        // 実際に使った点の角度
float scanHz = 0;
uint32_t goodPackets = 0;
uint32_t badPackets = 0;

// 1周のあいだに集めている途中の値
uint16_t candMm = 0;
uint8_t candIntensity = 0;
float candAngle = 0;
float candDiff = 999.0f;

// ------------------------------------------------------------ 受信用
uint8_t pkt[10 + 255 * 3];  // ヘッダ10バイト + 1点3バイト × 最大255点
int pktLen = 0;
int pktTotal = 0;

// 角度の差（0度をまたいでも正しく）。0〜180 を返す
float angleDiff(float a, float b) {
  float d = fabsf(a - b);
  while (d >= 360.0f) d -= 360.0f;
  return d > 180.0f ? 360.0f - d : d;
}

// 1周の区切り: 集めた値を結果に移す
void newRevolution() {
  frontMm = candMm;
  frontIntensity = candIntensity;
  frontAngle = candAngle;
  candMm = 0;
  candIntensity = 0;
  candDiff = 999.0f;
}

// 1点ぶん: 正面にいちばん近い点なら覚えておく
void usePoint(float angleDeg, uint16_t mm, uint8_t intensity) {
  if (mm < MIN_MM) return;
  float robotDeg = angleDeg - MOUNT_OFFSET_DEG;
  float d = angleDiff(robotDeg, FRONT_DEG);
  if (d <= WINDOW_DEG && d < candDiff) {
    candDiff = d;
    candMm = mm;
    candIntensity = intensity;
    candAngle = robotDeg < 0 ? robotDeg + 360.0f : robotDeg;
  }
}

// =============================================================
//  パケット1つぶんを読む
//  AA 55 | CT | LSN | FSA(2) | LSA(2) | CS(2) | 点(強度1+距離2) × LSN
// =============================================================
void handlePacket() {
  uint8_t ct = pkt[2];
  uint8_t lsn = pkt[3];
  uint16_t fsa = pkt[4] | (pkt[5] << 8);
  uint16_t lsa = pkt[6] | (pkt[7] << 8);
  uint16_t cs = pkt[8] | (pkt[9] << 8);

  // チェックサム
  uint16_t sum = 0x55AA ^ fsa;
  for (int i = 0; i < lsn; i++) {
    uint8_t* s = &pkt[10 + i * 3];
    sum ^= s[0];
    sum ^= s[1] | (s[2] << 8);
  }
  sum ^= (lsn << 8) | ct;
  sum ^= lsa;
  if (sum != cs) {
    badPackets++;
    return;
  }
  goodPackets++;

  // 1周の始まり
  if (ct & 0x01) {
    scanHz = (ct >> 1) / 10.0f;
    newRevolution();
  }

  // 角度
  float a0 = (fsa >> 1) / 64.0f;
  float a1 = (lsa >> 1) / 64.0f;
  float span = a1 - a0;
  if (span < 0) span += 360.0f;

  // 1点ずつ
  for (int i = 0; i < lsn; i++) {
    uint8_t* s = &pkt[10 + i * 3];
    uint8_t intensity = s[0];                 // 1バイト目 = 強度
    uint16_t mm = (s[1] | (s[2] << 8)) >> 2;  // 2〜3バイト目 = 距離
    float angle = (lsn > 1) ? a0 + span * i / (lsn - 1) : a0;
    if (angle >= 360.0f) angle -= 360.0f;
    usePoint(angle, mm, intensity);
  }
}

// 1バイト受け取るたびに呼ぶ
void handleByte(uint8_t b) {
  if (pktLen == 0) {
    if (b == 0xAA) pkt[pktLen++] = b;
    return;
  }
  if (pktLen == 1) {
    if (b == 0x55) pkt[pktLen++] = b;
    else if (b != 0xAA) pktLen = 0;
    return;
  }
  pkt[pktLen++] = b;
  if (pktLen == 10) {
    if (pkt[3] == 0) {
      pktLen = 0;
      return;
    }
    pktTotal = 10 + pkt[3] * 3;
  }
  if (pktLen >= 10 && pktLen == pktTotal) {
    handlePacket();
    pktLen = 0;
  }
}

// コマンド送信（A5 + 1バイト）
void sendCommand(uint8_t cmd) {
  uint8_t b[2] = {0xA5, cmd};
  Serial1.write(b, 2);
}

void startScan() {
  sendCommand(0x65);  // 停止
  delay(50);
  while (Serial1.available()) Serial1.read();
  pktLen = 0;
  sendCommand(0x60);  // スキャン開始
}

// =============================================================
//  setup / loop
// =============================================================
uint32_t lastGood = 0;
unsigned long lastDataMs = 0;
unsigned long lastPrintMs = 0;

void setup() {
  Serial.begin(115200);
  Serial1.setTX(PIN_LIDAR_TX);
  Serial1.setRX(PIN_LIDAR_RX);
  Serial1.setFIFOSize(4096);
  Serial1.begin(230400);
  delay(500);
  startScan();
  lastDataMs = millis();
}

void loop() {
  // 1. 届いたバイトを全部読む
  while (Serial1.available()) handleByte((uint8_t)Serial1.read());

  // 2. 1秒データが来なければ、開始コマンドを送り直す
  if (goodPackets != lastGood) {
    lastGood = goodPackets;
    lastDataMs = millis();
  } else if (millis() - lastDataMs > 1000) {
    Serial.println("データが来ません。開始コマンドを送り直します");
    startScan();
    lastDataMs = millis();
  }

  // 3. 0.5秒ごとに表示
  if (millis() - lastPrintMs >= 500) {
    lastPrintMs = millis();
    if (frontMm) {
      // 強度を棒グラフでも出す（# 1個 = 強度16）
      char bar[17];
      int n = frontIntensity / 16;
      for (int i = 0; i < 16; i++) bar[i] = i < n ? '#' : '.';
      bar[16] = '\0';
      Serial.printf("前: %5u mm  強度: %3u %s  (%.1f度)  %.1fHz\n", frontMm,
                    frontIntensity, bar, frontAngle, scanHz);
    } else {
      Serial.printf("前:   --- mm  強度: ---  %.1fHz  OK=%lu NG=%lu\n", scanHz,
                    (unsigned long)goodPackets, (unsigned long)badPackets);
    }
  }
}

読むところは4つです。

setFIFOSize(4096) が効いています。230400bps の線に、LiDAR は1秒に約1万3千バイト流してきます。受け取る場所が小さいと、loop() が少しよそ見をしただけで取りこぼします。4096 バイトは約0.3秒ぶんです。

delay() を使っていないことにも理由があります。0.5秒ごとの表示は millis() で測っています。delay(500) と書くと、そのあいだ受信が止まり、固まりが途中で切れます。

そしてデータが1秒来なければ、開始コマンドを送り直します。電源が一瞬落ちて LiDAR が止まっても、自分で復帰します。

ためしてみよう ― 強度で材質がわかる

画面には距離だけでなく 強度（0〜255）が棒グラフで出ます。跳ね返ってきた光の強さです。

正面に、白い紙・黒い紙・手・ペットボトルを、同じ距離で置きかえてみてください。距離はほとんど同じまま、強度だけが変わります。

迷路の壁は白です。競技で使う条件としては、いちばん測りやすい相手になります。

動かないときに見るところ ― 症状から原因を引く

次の記事 まわりの形が画面に出る で、360度ぶんの距離を Processing で絵にします。円・四角い箱・三角の中に入れると、点の集まりがそのまま形になります。