まわりの形が画面に出る ―
LiDAR の360度を見る

前の記事では、正面の1方向だけ距離を読みました。今回は360方向を全部受け取って、上から見た図にします。そのうえで、形のわかっている物の中に入れて、点が本当に壁のとおりに並ぶかを確かめます。

この記事は、正面までの距離を読む ― LiDAR をつなぐ の続きです。配線と、正面1方向の読み方はあちらにあります。パソコン側は Processing を使うので、Processing のはじめ方 も先に見ておいてください。

1周ぶんを、1行で送る

T-mini Plus は1秒に4000回測り、6回転します。1周ぶんは約667点です。

この667点を、1度ずつ360個の箱に入れます。1つの箱に入るのは平均1.9点。だいたいの箱が埋まります。

1周そろったら、箱の中身を1行で送ります。

D,150,150,151,152,…,150,6.0

1行は約2000文字、1秒に6行で 12KB ほどです。USB には余裕があります。

プログラム ― Pico 側

そのまま書き込めます。受信の部分は前の記事と同じで、変えたのは2か所だけです。

// =====================================================================
//  T-mini Plus の360度ぶんの距離を、1周ごとに1行で送るサンプル
//
//  1周そろうたびに、USB へ次の1行を出す。
//    D,距離0,距離1,…,距離359,回転数
//  距離はロボット基準の 0度（正面）から右回りに1度ずつ、単位は mm。
//  測れなかった方向は 0。
//
//  受け手は Processing の LidarView。
//  Arduino IDE のシリアルモニタで見てもよい（115200bps）。
//
//  ボード : Raspberry Pi Pico（Arduino IDE / earlephilhower コア）
//  配線   : LiDAR VCC → 5V（1A以上）   LiDAR Tx → GP1(2番)
//           LiDAR GND → GND            LiDAR Rx → GP0(1番)
//
//  受信の部分（handleByte / handlePacket）は FrontSample と同じ。
//  変えたのは、点のしまい方（work[360]）と、送り方だけ。
// =====================================================================
#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 uint16_t MIN_MM = 50;  // データシートの最短測距。これより近い値は捨てる

// ------------------------------------------------------------ 結果
uint16_t dist[360];  // 直前の1周。1度ずつ。0 = その方向は測れなかった
uint16_t work[360];  // いま集めている途中の1周
bool scanReady = false;
float scanHz = 0;
uint32_t goodPackets = 0;
uint32_t badPackets = 0;

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

// 1周の区切り: 集め終わった1周を結果に移し、次の1周ぶんを空にする
void newRevolution() {
  memcpy(dist, work, sizeof(dist));
  memset(work, 0, sizeof(work));
  scanReady = true;
}

// 1点ぶん: 近い1度の箱にしまう
void usePoint(float angleDeg, uint16_t mm) {
  if (mm < MIN_MM) return;
  float robotDeg = angleDeg - MOUNT_OFFSET_DEG;
  int i = (int)floorf(robotDeg + 0.5f) % 360;
  if (i < 0) i += 360;
  work[i] = mm;  // 同じ箱に2点入ったら、あとの点で上書きする
}

// =============================================================
//  パケット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];
    uint16_t mm = (s[1] | (s[2] << 8)) >> 2;
    float angle = (lsn > 1) ? a0 + span * i / (lsn - 1) : a0;
    if (angle >= 360.0f) angle -= 360.0f;
    usePoint(angle, mm);
  }
}

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

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) {
    startScan();
    lastDataMs = millis();
  }

  // 3. 1周そろったら、360個の距離を1行で送る
  if (scanReady) {
    scanReady = false;
    Serial.print('D');
    for (int i = 0; i < 360; i++) {
      Serial.print(',');
      Serial.print(dist[i]);
    }
    Serial.print(',');
    Serial.println(scanHz, 1);
  }
}

1周の区切りは、LiDAR が教えてくれます。パケットの CT の bit0 が 1 のとき、そこが1周の始まりです。そこで集め終わった work[] を dist[] に移し、work[] を空にします。

送っているあいだも、LiDAR はデータを送り続けています。LiDAR が流してくるのは毎秒13KB ほど。受け取る場所（setFIFOSize(4096)）は約0.3秒ぶんあるので、取りこぼしません。

プログラム ― Processing 側

送られてきた360個の距離を、上から見た図にします。角度 k 度・距離 mm の点は、画面では次の場所です。

float a = radians(k);
float x =  mm * pxPerMm * sin(a);
float y = -mm * pxPerMm * cos(a);

y に マイナス が付いているのは、画面の y が下向きだからです。0度を上（正面）にするために、符号を逆にしています。

// =====================================================================
//  LidarView ― T-mini Plus の距離を、上から見た図にする（Processing 4）
//
//  Pico に ScanToProcessing を書き込み、USB でつないでから実行する。
//  Arduino IDE のシリアルモニタは閉じておく（同じポートを2つで開けないため）。
//
//  操作
//    マウスのホイール / + -  … 拡大・縮小
//    g                      … 30cm のマス目（メイズのタイル）を出す・消す
//    l                      … となりの点を線でつなぐ・つながない
//    s                      … 今の画面を PNG で保存
// =====================================================================
import processing.serial.*;

Serial port;
String portName = "";
volatile int[] dist = new int[360];  // 最新の1周（mm）。0 は測れなかった方向
volatile float scanHz = 0;
int lineCount = 0;
int lastLineMs = 0;

float pxPerMm = 0.25;      // 1mm を何ピクセルで描くか
boolean showGrid = true;
boolean showLines = true;

void setup() {
  size(900, 900);
  pixelDensity(displayDensity());
  textFont(createFont("Hiragino Sans", 14));

  String[] ports = Serial.list();
  printArray(ports);
  for (String p : ports) {
    if (p.contains("usbmodem")) portName = p;   // Mac で Pico は /dev/cu.usbmodem…
  }
  if (portName.equals("") && ports.length > 0) portName = ports[0];
  if (!portName.equals("")) {
    port = new Serial(this, portName, 115200);
    port.bufferUntil('\n');
  }
}

// 1行届くたびに呼ばれる： D,距離0,…,距離359,回転数
void serialEvent(Serial p) {
  String line = p.readStringUntil('\n');
  if (line == null) return;
  line = trim(line);
  if (!line.startsWith("D,")) return;
  String[] t = split(line, ',');
  if (t.length != 362) return;              // 途中で切れた行は使わない
  int[] d = new int[360];
  for (int i = 0; i < 360; i++) d[i] = int(t[i + 1]);
  dist = d;                                  // 描いている途中で混ざらないよう、まとめて差し替える
  scanHz = float(t[361]);
  lineCount++;
  lastLineMs = millis();
}

void draw() {
  background(250);
  int[] d = dist;

  pushMatrix();
  translate(width / 2, height / 2);

  // 30cm のマス目。ロボットがマスの中央にいるとき、壁の位置と重なる
  if (showGrid) {
    stroke(232, 222, 200);
    strokeWeight(1);
    float half = max(width, height);
    for (int k = -30; k <= 30; k++) {
      float v = (150 + 300 * k) * pxPerMm;
      if (abs(v) > half) continue;
      line(v, -half, v, half);
      line(-half, v, half, v);
    }
  }

  // 50cm ごとの距離の円
  noFill();
  stroke(215);
  for (int r = 500; r <= 12000; r += 500) {
    float rr = r * pxPerMm;
    if (rr > width) break;
    ellipse(0, 0, rr * 2, rr * 2);
    fill(150);
    text((r / 10) + "cm", 4, -rr - 4);
    noFill();
  }

  // 測れた点。角度は正面（上）から右回り
  float px = 0, py = 0;
  boolean prevOk = false;
  for (int i = 0; i <= 360; i++) {
    int k = i % 360;
    int mm = d[k];
    if (mm <= 0) { prevOk = false; continue; }
    float a = radians(k);
    float x = mm * pxPerMm * sin(a);
    float y = -mm * pxPerMm * cos(a);
    // となりの点と 10cm 以内なら、同じ壁とみなして線でつなぐ
    if (showLines && prevOk && abs(mm - d[(k + 359) % 360]) < 100) {
      stroke(25, 113, 194, 160);
      strokeWeight(2);
      line(px, py, x, y);
    }
    stroke(25, 113, 194);
    strokeWeight(4);
    point(x, y);
    px = x; py = y; prevOk = true;
  }

  // ロボット（中心）と正面の向き
  fill(217, 115, 26);
  noStroke();
  ellipse(0, 0, 14, 14);
  stroke(217, 115, 26);
  strokeWeight(3);
  line(0, 0, 0, -30);
  fill(217, 115, 26);
  text("前", -7, -36);
  popMatrix();

  // 左上の文字
  fill(40);
  int y = 24;
  text("ポート: " + (portName.equals("") ? "見つかりません（Pico をつないで、もう一度実行）" : portName), 16, y); y += 22;
  boolean fresh = millis() - lastLineMs < 1000;
  text("回転数: " + nf(scanHz, 0, 1) + " Hz　受け取った周: " + lineCount + (fresh || lineCount == 0 ? "" : "（止まっています）"), 16, y); y += 22;
  text("前 " + near(d, 0) + "　右 " + near(d, 90) + "　後 " + near(d, 180) + "　左 " + near(d, 270), 16, y); y += 22;
  fill(120);
  text("ホイール/+ - 拡大　g マス目　l 線　s 保存", 16, height - 16);
}

// deg の前後 2° のうち、測れた点の中央値（cm で表示）。なければ —
String near(int[] d, int deg) {
  IntList v = new IntList();
  for (int k = -2; k <= 2; k++) {
    int mm = d[(deg + k + 360) % 360];
    if (mm > 0) v.append(mm);
  }
  if (v.size() == 0) return "—";
  v.sort();
  return nf(v.get(v.size() / 2) / 10.0, 0, 1) + "cm";
}

void mouseWheel(MouseEvent e) {
  pxPerMm = constrain(pxPerMm * pow(1.1, -e.getCount()), 0.03, 2.0);
}

void keyPressed() {
  if (key == '+' || key == '=') pxPerMm = min(pxPerMm * 1.2, 2.0);
  if (key == '-') pxPerMm = max(pxPerMm / 1.2, 0.03);
  if (key == 'g') showGrid = !showGrid;
  if (key == 'l') showLines = !showLines;
  if (key == 's') saveFrame("lidar-####.png");
}

となりの点を線でつなぐのは、距離の差が 10cm 以内のときだけです。差が大きいところは、別の物を見ている境目です。そこをつながないので、物の切れ目がそのまま切れ目として見えます。

形が出るか ― 円・四角・三角に閉じこめる

点が正しく並んでいるかは、形のわかっている物の中に入れてみるのがいちばん早い確かめ方です。板を丸めた輪、板で囲った四角、板を3枚立てた三角。この3つで試します。

以下は、実際に測った画面です。点のあいだは線でつないであります（l キー）。

円に入れて、中心に置いたとき。どの方向も同じ距離になります。中心からずらすと、ずらした向きだけ近くなります。

確かめの本体は、形ではなく上の行の数字です。円のまん中に置いたのだから、4方向は同じ値になるはずです。画面から9回読みとって、いちばん遠いのが 17.9cm、いちばん近いのが 17.3cm。ひらきは 6mm で、ここにはまん中に置いた精度もふくまれます。角度の割りふりも、距離の読み方も、おかしくはありません。形が円に見えるだけでは、そこまでは言えません。

四角い箱に入れたとき。壁は直線なので、正面がいちばん近く、角がいちばん遠くなります。垂直距離を D、壁の正面からの角度を θ とすると、距離は D ÷ cosθ です。

4つの角のすぐ外へ、点が何個か飛び出しています。壁と壁のつなぎ目にすき間があれば、その方向だけ光が奥まで抜けます。斜めに当たりすぎて返りが弱いときにも、遠めの値が出ます。角の数点は、信用しない。距離を使うときに1点だけを読まず、まわり何点かの真ん中の値をとるのは、これが理由です。

壁そのものも、数mm波打っています。まっすぐな板でも、点はきれいな直線にはなりません。

三角に入れたとき。遠い角が3つ、近い壁が3つ。四角との違いは、角の数だけです。

三角では、左だけが 17.7cm と、ほかより 5cm ほど遠くなっています。その方向に壁が無いからではなく、その方向が角だからです。前・右・後ろは壁、左は角。

大事なのは、1周ごとのゆれ（数mm）より、方向どうしの差のほうがずっと大きいことです。前 12.5 と 後 13.7 は 1cm 以上はなれていて、ゆれでは決してまぎれません。だから、数字を見るだけで自分がどちらを向いているかがわかります。

同じ数字を、横軸を角度にして並べると、もっとはっきりします。

点の間隔は、場所によって変わります。壁の上では正面がいちばん細かく、角へ行くほど粗くなります。間隔は 1 ÷ cos²θ に比例するので、四角の角（45度）では、正面の2倍の粗さです。

向きがずれていたら、MOUNT_OFFSET_DEG で直します。正面に手をかざして、点がどの角度に出るかを読み、その値を入れます。以後はロボットの正面が 0度になります。

センサーは黒くて丸く、見た目では向きがわかりません。上に矢印を1枚貼っておくと、ロボットに載せるときも、外して机で試すときも、同じ向きに置けます。MOUNT_OFFSET_DEG を一度決めたら、その値が使いつづけられるということです。

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

次は、壁との距離を一定に保ちながら走らせます。ここで読めるようになった距離を、そのままモーターの速さに変える話です。