測距者 8 路 × Arduino 範例

回到 8 路雷射測距首頁　｜　MBC 通用 I2C 協議（進階）

這一頁有 11 個產品範例（其中 5 個下命令改設定）＋3 個共用範例，從「確認接好了沒」一路做到「避障小車」。每個範例都可以直接複製、上傳，程式裡每一行都有中文說明。完全沒寫過程式也沒關係，照順序一個一個做就好。

提示

第一次用 Arduino？先看 🔰 第一次用 Arduino：從零開始，15 分鐘學會安裝軟體、上傳程式、看結果。

接線（所有範例都一樣）

圖：/images/figures/tof8/wiring-uno.svg

重要

• 請用 通用 I2C 版 的測距者 8 路。SPIKE 版、EV3 版的韌體不同，不能直接接 Arduino。
• 感應器一通電，要花 2～4 秒把 8 顆雷射一顆一顆叫醒。還沒就緒前距離都是 0，看起來像貼著障礙物，所以每個範例一開始都先等 F3 就緒（最多 6 秒），你自己寫程式時也要這樣做。
• 接線前先拔掉 USB 線。

8 顆雷射朝哪裡看？（感應器裝在車頭，站在車後往前看）

指令總覽表（協議 v2）

感應器像一排有編號的「格子」，每一格放一個 16 位元的數字（int16，小端）。讀法：寫 [編號] → 等 2 ms → 讀 2×k 個 byte，就能從那個編號起一次拿到連續 k 格（k ≤ 16）。沒有的編號會回 -32768。門牌號碼（I2C 位址）是 0x29。

下命令改設定（E0～E4）

提示

Arduino Nano 實測通過（2026-11-02）：E0～E4 每個命令在通用 I2C 版上機都照預期改變、讀回正確，超出範圍的參數也照規則夾住或不做。

怎麼送： 寫兩個 byte [命令碼][參數]（範例裡的 mbcCommand(0xE0, 1)）。E0、E2、E3 要重新設定雷射晶片，約 0.1～0.5 秒，這段時間狀態旗標 F3 的 bit2（忙碌）是 1，等它變 0 再讀；E1、E4 馬上生效。想知道現在的設定，讀 20～23。

• 參數超出範圍會被夾住：例如 E2 5 變 20、E2 250 變 200、一般模式 E4 250 變 120（遠距離 200）。不認得的參數不做：E0 9、E1 3、E3 不是 A5，感應器都不理，也不會出錯。
• 換模式（E0）會把更新時間和距離上限重設：要搭配 E2、E4，先送 E0 再送它們。
• 設定只存在感應器記憶體：斷電或重開機就回出廠（一般模式、上限 120 cm），程式每次開機要重新送。
• 「沒東西」讀到的是上限：一般模式 120、遠距離模式 200，用 E4 改過就是你設的上限。
• EV3、MATRIX 版沒有命令（只能讀）。

範例總覽

提示

建議順序：1 → 2 → 3 → 4 → 5 → 6，想改設定再看 7～11。 先確認接好（1），看懂每一格（2），再學方向（3、4）和應用。

1️⃣ 跟感應器打招呼

做什麼： 確認感應器接好了，並讀出它的身分證：F0 裝置 ID（出廠位址＋雷射顆數）、F1 協議版本、F2 韌體版本、F3 狀態旗標逐 bit 用中文印出來，再用 07 初始化位元圖列出 8 顆有沒有都開機成功。每次接新的感應器，都先跑這一個。

步驟：

1. 照上面的表格接好 4 條線，USB 插上電腦。
2. 複製下面的程式，貼到 Arduino IDE，按 → 上傳。
3. 打開序列埠監控窗（右上角放大鏡），速度選 115200。

下載 tof8_01_hello.ino

/*
 * 測距者 8 路 × Arduino　範例 1：跟感應器打招呼
 * ------------------------------------------------------------
 * 這個範例會做什麼？
 *   確認感應器有沒有接好。接好了，會印出它的身分證（裝置 ID、協議版本、
 *   韌體版本）和目前狀態（8 顆雷射是不是都正常）。
 *   每次接新感應器，都建議先跑這個範例。
 *
 * 你需要：測距者 8 路（通用 I2C 版）、Arduino UNO／Nano／UNO R4 或 ESP32。
 *
 * 接線：
 *   感應器 VCC → 5V      感應器 GND → GND
 *   感應器 SDA → A4      感應器 SCL → A5         （UNO / Nano）
 *   UNO R4 接 SDA / SCL 腳；ESP32 接 GPIO 21 (SDA) / GPIO 22 (SCL)
 *
 * 怎麼看結果：
 *   上傳後，打開「序列埠監控窗」，速度選 115200。
 *
 * 小提醒：
 *   8 顆雷射一顆一顆叫醒要 2～4 秒，還沒就緒前距離都是 0，所以一定要先等 F3 bit0。
 */

#include <Wire.h>                        // Arduino 內建的 I2C 通訊工具，用來跟感應器講話

const uint8_t ADDR = 0x29;               // 測距者的「門牌號碼」（I2C 位址），2 路與 8 路都是 0x29
const int16_t MBC_NONE = -32768;         // 「沒有這個值／讀不到」的代號

// 小幫手的宣告（實作在檔案最下面）；mbcWrite 的參數可以省略
bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p = NULL, uint8_t n = 0);

void setup() {
  Serial.begin(115200);                  // 打開跟電腦講話的通道，速度 115200
  mbcBegin();                            // 準備好 I2C 通訊（小幫手在檔案最下面）
  delay(500);                            // 等感應器開機 0.5 秒

  Serial.println(F("=== 測距者 8 路：跟感應器打招呼 ==="));
  Serial.println(F("等 8 顆雷射開機（最多 6 秒）..."));

  // 等感應器說「我準備好了」（狀態旗標 F3 的 bit0 = 8 顆都初始化完），最多等 6 秒
  if (!mbcWaitReady(ADDR, 6000)) {
    Serial.println(F("找不到感應器！請檢查："));
    Serial.println(F("  1. SDA 接 A4、SCL 接 A5（ESP32：21 / 22），有沒有接反？"));
    Serial.println(F("  2. VCC 接 5V、GND 接 GND，有沒有插緊？"));
    Serial.println(F("  3. 你的感應器是「通用 I2C 版」嗎？（SPIKE 版、EV3 版不能接 Arduino）"));
    while (true) {}                      // 找不到就停在這裡
  }

  // ---- F0 裝置 ID：低 byte = 出廠位址、高 byte = 雷射顆數 ----
  int16_t id = mbcRead(ADDR, 0xF0);
  uint8_t factoryAddr = (uint8_t)(id & 0xFF);        // 出廠位址（換過位址也不變）
  uint8_t sensors     = (uint8_t)((id >> 8) & 0xFF); // hw[0] = 幾顆雷射
  Serial.print(F("找到了！出廠位址 0x"));
  Serial.print(factoryAddr, HEX);
  Serial.print(F("，雷射數量 "));
  Serial.print(sensors);                 // 測距者 8 路 = 8
  Serial.println(F(" 顆"));

  // ---- F1 協議版本（這份範例是照版本 2 寫的） ----
  Serial.print(F("協議版本："));
  Serial.println(mbcRead(ADDR, 0xF1));

  // ---- F2 韌體版本：主版×100 + 次版（例如 102 = 1.2） ----
  int16_t fw = mbcRead(ADDR, 0xF2);
  Serial.print(F("韌體版本："));
  Serial.print(fw / 100);
  Serial.print('.');
  Serial.println(fw % 100);

  // ---- F3 狀態旗標：一個 bit 代表一件事 ----
  int16_t st = mbcRead(ADDR, 0xF3);
  Serial.println(F("目前狀態："));
  Serial.print(F("  bit0 就緒        ")); Serial.println((st & 0x01) ? F("是（8 顆都初始化完）") : F("否"));
  Serial.print(F("  bit1 任一顆失效  ")); Serial.println((st & 0x02) ? F("是！（跑範例 2 看是哪一顆）") : F("否（8 顆都正常）"));

  // ---- 07 初始化位元圖：bit0 = 第 1 顆（最左）… bit7 = 第 8 顆 ----
  int16_t initOk = mbcRead(ADDR, 0x07);
  Serial.print(F("初始化成功的雷射（左→右）："));
  for (uint8_t i = 0; i < 8; i++) Serial.print((initOk >> i) & 1 ? 'O' : 'X');
  Serial.println(initOk == 255 ? F("  全部成功") : F("  有 X 的那顆開機失敗，請重新上電"));

  // ---- F5 開機秒數 ----
  Serial.print(F("開機秒數："));
  Serial.println(mbcRead(ADDR, 0xF5));

  Serial.println(F("接線正確，可以開始玩其他範例了！"));
}

void loop() {
  // 這個範例只要在開機時檢查一次，所以 loop 裡什麼都不做
}

// ============================================================
// 以下是「通訊小幫手」（MBC I2C 協議 v2）：負責跟感應器講話。
// 所有 MBC 範例都一樣，不用改，也不用看懂。
//   讀法：寫 [編號] → 等 2 ms → 讀 2×k 個 byte = 從該編號起連續 k 個 int16（小端）
//   沒有這個編號（空號）時感應器回 -32768（MBC_NONE）
// ============================================================

// 開始 I2C 通訊（setup 裡呼叫一次）
// UNO/Nano：SDA=A4、SCL=A5；UNO R4：SDA/SCL 腳；ESP32：預設 GPIO21(SDA)/22(SCL)
void mbcBegin() {
  Wire.begin();
#if defined(WIRE_HAS_TIMEOUT)
  Wire.setWireTimeout(25000, true);      // 線沒接好時，不會讓 Arduino 當機卡住
#endif
}

// 從位址 addr 的感應器，由編號 id 起連續讀 k 個值（1~16）放進 out。全部讀到回傳 true
bool mbcReadMany(uint8_t addr, uint8_t id, uint8_t k, int16_t *out) {
  if (k < 1) k = 1;
  if (k > 16) k = 16;                    // 一次最多 16 個值（32 byte，Wire 緩衝區上限）
  Wire.beginTransmission(addr);
  Wire.write(id);                        // 告訴感應器：我要從第 id 號開始讀
  if (Wire.endTransmission() != 0) return false;
  delay(2);                              // 等 2 ms 再讀（ESP32 型的產品一定要等）
  uint8_t n = (uint8_t)(2 * k);
  if (Wire.requestFrom(addr, n) != n) return false;
  for (uint8_t i = 0; i < k; i++) {
    uint16_t lo = Wire.read();           // 先來的是低位元組
    uint16_t hi = Wire.read();           // 再來高位元組（小端）
    out[i] = (int16_t)(uint16_t)(lo | (hi << 8));
  }
  return true;
}

// 讀一個值。讀不到（沒接好）回傳 MBC_NONE
int16_t mbcRead(uint8_t addr, uint8_t id) {
  int16_t v;
  return mbcReadMany(addr, id, 1, &v) ? v : MBC_NONE;
}

// 送命令：寫 [id][參數…]。p 指向參數、n = 參數幾個 byte（沒參數就不填）。成功回傳 true
bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p, uint8_t n) {
  Wire.beginTransmission(addr);
  Wire.write(id);
  for (uint8_t i = 0; i < n; i++) Wire.write(p[i]);
  return Wire.endTransmission() == 0;
}

// 等感應器準備好（狀態旗標 F3 的 bit0 = 1），最多等 ms 毫秒。準備好了回傳 true
bool mbcWaitReady(uint8_t addr, uint16_t ms) {
  unsigned long start = millis();
  while (millis() - start < ms) {
    int16_t f = mbcRead(addr, 0xF3);
    if (f != MBC_NONE && (f & 1)) return true;
    delay(50);
  }
  return false;
}

你會看到：

=== 測距者 8 路：跟感應器打招呼 ===
等 8 顆雷射開機（最多 6 秒）...
找到了！出廠位址 0x29，雷射數量 8 顆
協議版本：2
韌體版本：2.0
目前狀態：
  bit0 就緒        是（8 顆都初始化完）
  bit1 任一顆失效  否（8 顆都正常）
初始化成功的雷射（左→右）：OOOOOOOO  全部成功
開機秒數：6
接線正確，可以開始玩其他範例了！

注意

「通訊小幫手」是什麼？ 每個範例最下面都有一段一模一樣的程式，負責跟感應器「講話」。你不用改它，也不用看懂。只要記得：

• mbcRead(位址, 編號)：讀一格，讀不到回 MBC_NONE（−32768）
• mbcReadMany(位址, 起始編號, 幾格, 陣列)：從起始編號一次讀連續幾格（最多 16），全部來自同一瞬間
• mbcWrite(位址, 命令, 參數, 參數幾個)：送命令，沒參數時後兩個可以不寫
• mbcWaitReady(位址, 最多等幾毫秒)：等 F3 bit0 就緒

2️⃣ 一次看完所有讀值

做什麼： 每 0.5 秒把整張值表印一次，分三段 mbcReadMany：01～09、10～17（8 顆 cm）、18～1F（8 顆 mm）。先在這裡認識每一格，之後的範例只是各挑幾格來用。

下載 tof8_02_all_values.ino

/*
 * 測距者 8 路 × Arduino　範例 2：一次看完所有讀值
 * ------------------------------------------------------------
 * 這個範例會做什麼？
 *   把感應器「整張值表」每 0.5 秒印一次：
 *     01 最近 cm、02 最近方向、03 最空曠 cm、04 最空曠方向、05 正前方 cm、
 *     06 最近 mm、07 初始化位元圖、08 故障位元圖、09 正前方 mm、
 *     10~17 第 1~8 顆 cm、18~1F 第 1~8 顆 mm。
 *   之後的範例只是各挑其中幾格來用，先在這裡把每一格認識一遍。
 *
 * 你需要：測距者 8 路（通用 I2C 版）＋ 你的手或幾本書。
 *
 * 接線：
 *   感應器 VCC → 5V      感應器 GND → GND
 *   感應器 SDA → A4      感應器 SCL → A5         （UNO / Nano）
 *   UNO R4 接 SDA / SCL 腳；ESP32 接 GPIO 21 (SDA) / GPIO 22 (SCL)
 *
 * 怎麼看結果：
 *   序列埠監控窗 115200。手從左邊慢慢移到右邊，看數字怎麼從第 1 顆走到第 8 顆。
 *
 * 小提醒：
 *   一次最多讀 16 個，所以分三段：01~09、10~17、18~1F。
 *   120 cm／1200 mm = 那個方向沒東西（或那顆失效）。
 */

#include <Wire.h>

const uint8_t ADDR = 0x29;               // 測距者的門牌號碼（I2C 位址）
const int16_t MBC_NONE = -32768;         // 「沒有這個值／讀不到」的代號

bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p = NULL, uint8_t n = 0);

void setup() {
  Serial.begin(115200);
  mbcBegin();
  delay(500);
  if (!mbcWaitReady(ADDR, 6000)) {
    Serial.println(F("找不到感應器，請先跑範例 1 檢查接線。"));
    while (true) {}
  }
  Serial.println(F("準備好了！拿手在感應器前面移動看看。"));
}

void loop() {
  int16_t a[9];                          // 01~09
  int16_t cm[8];                         // 10~17 第 1~8 顆 cm
  int16_t mm[8];                         // 18~1F 第 1~8 顆 mm
  if (!mbcReadMany(ADDR, 0x01, 9, a))  { Serial.println(F("讀不到（01~09）")); delay(500); return; }
  if (!mbcReadMany(ADDR, 0x10, 8, cm)) { Serial.println(F("讀不到（10~17）")); delay(500); return; }
  if (!mbcReadMany(ADDR, 0x18, 8, mm)) { Serial.println(F("讀不到（18~1F）")); delay(500); return; }

  Serial.println(F("---------------------------------------"));
  Serial.print(F("01 最近距離     ")); Serial.print(a[0]); Serial.println(F(" cm   （8 顆裡最小的；120 = 四周沒東西）"));
  Serial.print(F("02 最近方向     ")); Serial.print(a[1]); Serial.println(F("      （-7 正左 ~ +7 正右，0 = 正前方；±8 = 沒東西）"));
  Serial.print(F("03 最空曠距離   ")); Serial.print(a[2]); Serial.println(F(" cm   （8 顆裡最大的）"));
  Serial.print(F("04 最空曠方向   ")); Serial.print(a[3]); Serial.println(F("      （-7 ~ +7）"));
  Serial.print(F("05 正前方距離   ")); Serial.print(a[4]); Serial.println(F(" cm   （第 4、5 顆平均）"));
  Serial.print(F("06 最近距離     ")); Serial.print(a[5]); Serial.println(F(" mm   （比 cm 精細 10 倍）"));
  Serial.print(F("07 初始化位元圖 "));
  for (uint8_t i = 0; i < 8; i++) Serial.print((a[6] >> i) & 1);     // 左邊印 bit0（第 1 顆）
  Serial.println(F("  （左→右，1 = 開機成功）"));
  Serial.print(F("08 故障位元圖   "));
  for (uint8_t i = 0; i < 8; i++) Serial.print((a[7] >> i) & 1);
  Serial.println(F("  （左→右，1 = 那顆現在失效）"));
  Serial.print(F("09 正前方距離   ")); Serial.print(a[8]); Serial.println(F(" mm"));

  Serial.print(F("10~17 各顆 cm 左 "));
  for (uint8_t i = 0; i < 8; i++) { printPad(cm[i], 4); Serial.print(' '); }
  Serial.println(F(" 右"));
  Serial.print(F("18~1F 各顆 mm 左 "));
  for (uint8_t i = 0; i < 8; i++) { printPad(mm[i], 4); Serial.print(' '); }
  Serial.println(F(" 右   （第 1 顆正左 90°、第 4/5 顆正前、第 8 顆正右 90°）"));

  delay(50);
  delay(450);                            // 每 0.5 秒更新一次
}

// 靠右對齊印數字（寬度 w）
void printPad(int16_t v, uint8_t w) {
  uint8_t len = (v < 0) ? 2 : 1;
  for (int16_t t = (v < 0 ? -v : v); t >= 10; t /= 10) len++;
  for (uint8_t i = len; i < w; i++) Serial.print(' ');
  Serial.print(v);
}

// ============================================================
// 以下是「通訊小幫手」（MBC I2C 協議 v2）：負責跟感應器講話。
// 所有 MBC 範例都一樣，不用改，也不用看懂。
//   讀法：寫 [編號] → 等 2 ms → 讀 2×k 個 byte = 從該編號起連續 k 個 int16（小端）
//   沒有這個編號（空號）時感應器回 -32768（MBC_NONE）
// ============================================================

// 開始 I2C 通訊（setup 裡呼叫一次）
// UNO/Nano：SDA=A4、SCL=A5；UNO R4：SDA/SCL 腳；ESP32：預設 GPIO21(SDA)/22(SCL)
void mbcBegin() {
  Wire.begin();
#if defined(WIRE_HAS_TIMEOUT)
  Wire.setWireTimeout(25000, true);      // 線沒接好時，不會讓 Arduino 當機卡住
#endif
}

// 從位址 addr 的感應器，由編號 id 起連續讀 k 個值（1~16）放進 out。全部讀到回傳 true
bool mbcReadMany(uint8_t addr, uint8_t id, uint8_t k, int16_t *out) {
  if (k < 1) k = 1;
  if (k > 16) k = 16;                    // 一次最多 16 個值（32 byte，Wire 緩衝區上限）
  Wire.beginTransmission(addr);
  Wire.write(id);                        // 告訴感應器：我要從第 id 號開始讀
  if (Wire.endTransmission() != 0) return false;
  delay(2);                              // 等 2 ms 再讀（ESP32 型的產品一定要等）
  uint8_t n = (uint8_t)(2 * k);
  if (Wire.requestFrom(addr, n) != n) return false;
  for (uint8_t i = 0; i < k; i++) {
    uint16_t lo = Wire.read();           // 先來的是低位元組
    uint16_t hi = Wire.read();           // 再來高位元組（小端）
    out[i] = (int16_t)(uint16_t)(lo | (hi << 8));
  }
  return true;
}

// 讀一個值。讀不到（沒接好）回傳 MBC_NONE
int16_t mbcRead(uint8_t addr, uint8_t id) {
  int16_t v;
  return mbcReadMany(addr, id, 1, &v) ? v : MBC_NONE;
}

// 送命令：寫 [id][參數…]。p 指向參數、n = 參數幾個 byte（沒參數就不填）。成功回傳 true
bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p, uint8_t n) {
  Wire.beginTransmission(addr);
  Wire.write(id);
  for (uint8_t i = 0; i < n; i++) Wire.write(p[i]);
  return Wire.endTransmission() == 0;
}

// 等感應器準備好（狀態旗標 F3 的 bit0 = 1），最多等 ms 毫秒。準備好了回傳 true
bool mbcWaitReady(uint8_t addr, uint16_t ms) {
  unsigned long start = millis();
  while (millis() - start < ms) {
    int16_t f = mbcRead(addr, 0xF3);
    if (f != MBC_NONE && (f & 1)) return true;
    delay(50);
  }
  return false;
}

你會看到：（手放在左前方 30 公分）

---------------------------------------
01 最近距離     30 cm   （8 顆裡最小的；120 = 四周沒東西）
02 最近方向     -3      （-7 正左 ~ +7 正右，0 = 正前方；±8 = 沒東西）
03 最空曠距離   120 cm   （8 顆裡最大的）
04 最空曠方向   5      （-7 ~ +7）
05 正前方距離   120 cm   （第 4、5 顆平均）
06 最近距離     304 mm   （比 cm 精細 10 倍）
07 初始化位元圖 11111111  （左→右，1 = 開機成功）
08 故障位元圖   00000000  （左→右，1 = 那顆現在失效）
09 正前方距離   1200 mm
10~17 各顆 cm 左  120  120   30   85  120  120  120  120  右
18~1F 各顆 mm 左 1200 1200  304  851 1200 1200 1200 1200  右   （第 1 顆正左 90°、第 4/5 顆正前、第 8 顆正右 90°）

提示

試試看： 把手從左邊慢慢移到右邊，看數字是怎麼從第 1 顆「走」到第 8 顆的。第 4、5 顆都朝正前方，所以手在正前方時兩顆的數字會很接近。

3️⃣ 最近的東西在哪個方向？

做什麼： 感應器會自己比較 8 顆雷射：01 最近距離 cm、02 最近方向、06 最近距離 mm。電腦畫一把小尺用 * 標出方向，再用箭頭說出來。相撲機器人「找對手」用的就是這個。

準備： 一個盒子或你的手。

下載 tof8_03_nearest_direction.ino

/*
 * 測距者 8 路 × Arduino　範例 3：最近的東西在哪個方向？
 * ------------------------------------------------------------
 * 這個範例會做什麼？
 *   感應器會自己比較 8 顆雷射，告訴你最近的東西在哪個方向、有多遠：
 *     01 最近距離 cm、02 最近方向（-7 正左 ~ +7 正右，0 = 正前方）、06 最近距離 mm。
 *   電腦畫一把小尺用 * 標出方向，再用箭頭說出來。相撲機器人「找對手」用的就是這個。
 *
 * 你需要：測距者 8 路（通用 I2C 版）＋ 一個盒子或你的手。
 *
 * 接線：
 *   感應器 VCC → 5V      感應器 GND → GND
 *   感應器 SDA → A4      感應器 SCL → A5         （UNO / Nano）
 *   UNO R4 接 SDA / SCL 腳；ESP32 接 GPIO 21 (SDA) / GPIO 22 (SCL)
 *
 * 怎麼看結果：
 *   序列埠監控窗 115200。手從左前方移到正前方，再移到右邊。
 *
 * 小提醒：
 *   120 公分內什麼都沒有時，方向會變成 ±8（代表「上次偏哪一邊」），不是真的有東西。
 *   所以一定要先看距離再看方向。
 */

#include <Wire.h>

const uint8_t ADDR = 0x29;               // 測距者的門牌號碼（I2C 位址）
const int16_t MBC_NONE = -32768;         // 「沒有這個值／讀不到」的代號

bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p = NULL, uint8_t n = 0);

void setup() {
  Serial.begin(115200);
  mbcBegin();
  delay(500);
  if (!mbcWaitReady(ADDR, 6000)) {
    Serial.println(F("找不到感應器，請先跑範例 1 檢查接線。"));
    while (true) {}
  }
  Serial.println(F("準備好了！拿東西在感應器周圍移動看看。"));
}

void loop() {
  int16_t v[6];                          // 01 最近 cm、02 最近方向、03、04、05、06 最近 mm
  if (!mbcReadMany(ADDR, 0x01, 6, v)) { Serial.println(F("讀不到感應器")); delay(500); return; }
  int16_t nearCm = v[0], dir = v[1], nearMm = v[5];

  // 畫一把 15 格的小尺：方向 -7 ~ +7 各一格，* = 最近的東西，+ = 正前方
  Serial.print(F("左 ["));
  for (int8_t i = -7; i <= 7; i++) {
    if (nearCm < 120 && i == dir) Serial.print('*');
    else if (i == 0)              Serial.print('+');
    else                          Serial.print('.');
  }
  Serial.print(F("] 右   "));

  if (nearCm >= 120) {                   // 先看距離！沒東西時方向是 ±8，不能信
    Serial.println(F("前面 120 公分內沒有東西"));
  } else {
    if      (dir <= -6) Serial.print(F("← 左邊  "));
    else if (dir <= -2) Serial.print(F("↖ 左前方"));
    else if (dir <=  1) Serial.print(F("↑ 正前方"));
    else if (dir <=  5) Serial.print(F("↗ 右前方"));
    else                Serial.print(F("→ 右邊  "));
    Serial.print(' '); Serial.print(nearCm); Serial.print(F(" 公分（"));
    Serial.print(nearMm); Serial.print(F(" mm，方向 ")); Serial.print(dir); Serial.println(')');
  }
  delay(50);
  delay(150);                            // 每 0.2 秒印一行   👉 可以改
}

// ============================================================
// 以下是「通訊小幫手」（MBC I2C 協議 v2）：負責跟感應器講話。
// 所有 MBC 範例都一樣，不用改，也不用看懂。
//   讀法：寫 [編號] → 等 2 ms → 讀 2×k 個 byte = 從該編號起連續 k 個 int16（小端）
//   沒有這個編號（空號）時感應器回 -32768（MBC_NONE）
// ============================================================

// 開始 I2C 通訊（setup 裡呼叫一次）
// UNO/Nano：SDA=A4、SCL=A5；UNO R4：SDA/SCL 腳；ESP32：預設 GPIO21(SDA)/22(SCL)
void mbcBegin() {
  Wire.begin();
#if defined(WIRE_HAS_TIMEOUT)
  Wire.setWireTimeout(25000, true);      // 線沒接好時，不會讓 Arduino 當機卡住
#endif
}

// 從位址 addr 的感應器，由編號 id 起連續讀 k 個值（1~16）放進 out。全部讀到回傳 true
bool mbcReadMany(uint8_t addr, uint8_t id, uint8_t k, int16_t *out) {
  if (k < 1) k = 1;
  if (k > 16) k = 16;                    // 一次最多 16 個值（32 byte，Wire 緩衝區上限）
  Wire.beginTransmission(addr);
  Wire.write(id);                        // 告訴感應器：我要從第 id 號開始讀
  if (Wire.endTransmission() != 0) return false;
  delay(2);                              // 等 2 ms 再讀（ESP32 型的產品一定要等）
  uint8_t n = (uint8_t)(2 * k);
  if (Wire.requestFrom(addr, n) != n) return false;
  for (uint8_t i = 0; i < k; i++) {
    uint16_t lo = Wire.read();           // 先來的是低位元組
    uint16_t hi = Wire.read();           // 再來高位元組（小端）
    out[i] = (int16_t)(uint16_t)(lo | (hi << 8));
  }
  return true;
}

// 讀一個值。讀不到（沒接好）回傳 MBC_NONE
int16_t mbcRead(uint8_t addr, uint8_t id) {
  int16_t v;
  return mbcReadMany(addr, id, 1, &v) ? v : MBC_NONE;
}

// 送命令：寫 [id][參數…]。p 指向參數、n = 參數幾個 byte（沒參數就不填）。成功回傳 true
bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p, uint8_t n) {
  Wire.beginTransmission(addr);
  Wire.write(id);
  for (uint8_t i = 0; i < n; i++) Wire.write(p[i]);
  return Wire.endTransmission() == 0;
}

// 等感應器準備好（狀態旗標 F3 的 bit0 = 1），最多等 ms 毫秒。準備好了回傳 true
bool mbcWaitReady(uint8_t addr, uint16_t ms) {
  unsigned long start = millis();
  while (millis() - start < ms) {
    int16_t f = mbcRead(addr, 0xF3);
    if (f != MBC_NONE && (f & 1)) return true;
    delay(50);
  }
  return false;
}

你會看到：（手從左前方移到正前方，再移到右邊）

左 [.......+.......] 右   前面 120 公分內沒有東西
左 [..*....+.......] 右   ↖ 左前方 35 公分（352 mm，方向 -5）
左 [.......*.......] 右   ↑ 正前方 28 公分（283 mm，方向 0）
左 [.......+.....*.] 右   → 右邊   40 公分（401 mm，方向 6）

方向是什麼意思？

警告

一定要先看距離，再看方向。 120 公分內什麼都沒有時，方向是 ±8，不是真的有東西在那裡。範例裡先用「距離 < 120」確定有東西，才去看方向。

4️⃣ 哪邊最空曠？

做什麼： 讀 03 最空曠距離、04 最空曠方向——感應器幫你找出 8 個方向裡最遠（最沒東西）的那個。迷宮、避障時「往最空的地方走」就用這兩格。同時印 01／02 最近的，讓你比較「該躲哪裡、該往哪裡走」。

準備： 幾本書當牆壁。

下載 tof8_04_open_direction.ino

/*
 * 測距者 8 路 × Arduino　範例 4：哪邊最空曠？
 * ------------------------------------------------------------
 * 這個範例會做什麼？
 *   讀「03 最空曠距離 cm」「04 最空曠方向」：感應器幫你找出 8 個方向裡
 *   最遠（最沒東西）的那個方向。迷宮、避障時「往最空的地方走」就用這兩格。
 *   同時讀 01/02 最近的，兩個一起印，讓你比較「該躲哪裡、該往哪裡走」。
 *
 * 你需要：測距者 8 路（通用 I2C 版）＋ 幾本書當牆壁。
 *
 * 接線：
 *   感應器 VCC → 5V      感應器 GND → GND
 *   感應器 SDA → A4      感應器 SCL → A5         （UNO / Nano）
 *   UNO R4 接 SDA / SCL 腳；ESP32 接 GPIO 21 (SDA) / GPIO 22 (SCL)
 *
 * 怎麼看結果：
 *   序列埠監控窗 115200。用書在感應器左、右、前方擺出不同的「牆」。
 *
 * 小提醒：
 *   壞掉的那顆距離永遠是 120，會被當成「最空曠」；比賽前先跑範例 1 確認 8 顆都正常。
 */

#include <Wire.h>

const uint8_t ADDR = 0x29;               // 測距者的門牌號碼（I2C 位址）
const int16_t MBC_NONE = -32768;         // 「沒有這個值／讀不到」的代號

bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p = NULL, uint8_t n = 0);

void setup() {
  Serial.begin(115200);
  mbcBegin();
  delay(500);
  if (!mbcWaitReady(ADDR, 6000)) {
    Serial.println(F("找不到感應器，請先跑範例 1 檢查接線。"));
    while (true) {}
  }
  Serial.println(F("準備好了！用書在感應器旁邊擺幾面牆看看。"));
}

void loop() {
  int16_t v[4];                          // 01 最近 cm、02 最近方向、03 最空曠 cm、04 最空曠方向
  if (!mbcReadMany(ADDR, 0x01, 4, v)) { Serial.println(F("讀不到感應器")); delay(500); return; }
  int16_t nearCm = v[0], nearDir = v[1], farCm = v[2], farDir = v[3];

  Serial.print(F("最空曠 → "));
  printDir(farDir);
  Serial.print(' '); Serial.print(farCm); Serial.print(F(" 公分"));

  Serial.print(F("   ｜   最近 → "));
  if (nearCm >= 120) Serial.println(F("四周 120 公分內沒東西"));
  else {
    printDir(nearDir);
    Serial.print(' '); Serial.print(nearCm); Serial.println(F(" 公分"));
  }
  delay(50);
  delay(250);                            // 每 0.3 秒印一行   👉 可以改
}

// 把 -7 ~ +7 的方向印成文字（每一格 ≈ 13°，±7 ≈ ±90°）
void printDir(int16_t dir) {
  if      (dir <= -6) Serial.print(F("正左"));
  else if (dir <= -2) Serial.print(F("左前"));
  else if (dir <=  1) Serial.print(F("正前"));
  else if (dir <=  5) Serial.print(F("右前"));
  else                Serial.print(F("正右"));
  Serial.print(F("(")); Serial.print(dir); Serial.print(')');
}

// ============================================================
// 以下是「通訊小幫手」（MBC I2C 協議 v2）：負責跟感應器講話。
// 所有 MBC 範例都一樣，不用改，也不用看懂。
//   讀法：寫 [編號] → 等 2 ms → 讀 2×k 個 byte = 從該編號起連續 k 個 int16（小端）
//   沒有這個編號（空號）時感應器回 -32768（MBC_NONE）
// ============================================================

// 開始 I2C 通訊（setup 裡呼叫一次）
// UNO/Nano：SDA=A4、SCL=A5；UNO R4：SDA/SCL 腳；ESP32：預設 GPIO21(SDA)/22(SCL)
void mbcBegin() {
  Wire.begin();
#if defined(WIRE_HAS_TIMEOUT)
  Wire.setWireTimeout(25000, true);      // 線沒接好時，不會讓 Arduino 當機卡住
#endif
}

// 從位址 addr 的感應器，由編號 id 起連續讀 k 個值（1~16）放進 out。全部讀到回傳 true
bool mbcReadMany(uint8_t addr, uint8_t id, uint8_t k, int16_t *out) {
  if (k < 1) k = 1;
  if (k > 16) k = 16;                    // 一次最多 16 個值（32 byte，Wire 緩衝區上限）
  Wire.beginTransmission(addr);
  Wire.write(id);                        // 告訴感應器：我要從第 id 號開始讀
  if (Wire.endTransmission() != 0) return false;
  delay(2);                              // 等 2 ms 再讀（ESP32 型的產品一定要等）
  uint8_t n = (uint8_t)(2 * k);
  if (Wire.requestFrom(addr, n) != n) return false;
  for (uint8_t i = 0; i < k; i++) {
    uint16_t lo = Wire.read();           // 先來的是低位元組
    uint16_t hi = Wire.read();           // 再來高位元組（小端）
    out[i] = (int16_t)(uint16_t)(lo | (hi << 8));
  }
  return true;
}

// 讀一個值。讀不到（沒接好）回傳 MBC_NONE
int16_t mbcRead(uint8_t addr, uint8_t id) {
  int16_t v;
  return mbcReadMany(addr, id, 1, &v) ? v : MBC_NONE;
}

// 送命令：寫 [id][參數…]。p 指向參數、n = 參數幾個 byte（沒參數就不填）。成功回傳 true
bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p, uint8_t n) {
  Wire.beginTransmission(addr);
  Wire.write(id);
  for (uint8_t i = 0; i < n; i++) Wire.write(p[i]);
  return Wire.endTransmission() == 0;
}

// 等感應器準備好（狀態旗標 F3 的 bit0 = 1），最多等 ms 毫秒。準備好了回傳 true
bool mbcWaitReady(uint8_t addr, uint16_t ms) {
  unsigned long start = millis();
  while (millis() - start < ms) {
    int16_t f = mbcRead(addr, 0xF3);
    if (f != MBC_NONE && (f & 1)) return true;
    delay(50);
  }
  return false;
}

你會看到：（左邊和前面有書，右邊空的）

最空曠 → 正右(7) 120 公分   ｜   最近 → 正前(0) 18 公分
最空曠 → 右前(4) 95 公分   ｜   最近 → 左前(-4) 22 公分

提示

壞掉的那顆距離永遠是 120，會被當成「最空曠」。比賽前先跑 範例 1 確認 8 顆都正常。

5️⃣ 雷達長條圖

做什麼： 一次讀 10～17 八顆 cm，畫成 8 條長條：長條越長 = 那個方向越空曠，很短 = 有東西擋著。順便讀 08 故障位元圖，壞掉的那顆標 XX。看一眼就知道哪邊可以走。

準備： 幾本書當障礙物。

下載 tof8_05_radar_bars.ino

/*
 * 測距者 8 路 × Arduino　範例 5：雷達長條圖
 * ------------------------------------------------------------
 * 這個範例會做什麼？
 *   一次讀「10~17 第 1~8 顆距離 cm」，把 8 個方向畫成 8 條長條：
 *   長條越長 = 那個方向越空曠，很短 = 有東西擋著。看一眼就知道哪邊可以走。
 *   順便讀「08 故障位元圖」，壞掉的那顆標 XX。
 *
 * 你需要：測距者 8 路（通用 I2C 版）＋ 幾本書當障礙物。
 *
 * 接線：
 *   感應器 VCC → 5V      感應器 GND → GND
 *   感應器 SDA → A4      感應器 SCL → A5         （UNO / Nano）
 *   UNO R4 接 SDA / SCL 腳；ESP32 接 GPIO 21 (SDA) / GPIO 22 (SCL)
 *
 * 怎麼看結果：
 *   序列埠監控窗 115200。
 *
 * 8 顆雷射朝哪裡看？（站在車後往前看）
 *   第 1 顆 左 90°、第 2 顆 左 60°、第 3 顆 左 30°、第 4、5 顆 正前方、
 *   第 6 顆 右 30°、第 7 顆 右 60°、第 8 顆 右 90°
 */

#include <Wire.h>

const uint8_t ADDR = 0x29;               // 測距者的門牌號碼（I2C 位址）
const int16_t MBC_NONE = -32768;         // 「沒有這個值／讀不到」的代號

bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p = NULL, uint8_t n = 0);

const int8_t ANGLE[8] = {-90, -60, -30, 0, 0, 30, 60, 90};   // 每顆的角度（負 = 左）

void setup() {
  Serial.begin(115200);
  mbcBegin();
  delay(500);
  if (!mbcWaitReady(ADDR, 6000)) {
    Serial.println(F("找不到感應器，請先跑範例 1 檢查接線。"));
    while (true) {}
  }
  Serial.println(F("準備好了！放幾本書在感應器旁邊看看。"));
}

void loop() {
  int16_t cm[8];
  int16_t fault = mbcRead(ADDR, 0x08);   // 08 故障位元圖：bit0 = 第 1 顆
  if (!mbcReadMany(ADDR, 0x10, 8, cm) || fault == MBC_NONE) { Serial.println(F("讀不到感應器")); delay(500); return; }

  Serial.println(F("====== 雷達畫面（長條越長 = 越空曠）======"));
  for (uint8_t i = 0; i < 8; i++) {
    Serial.print(i + 1);                 // 第幾顆（1~8）
    Serial.print(' ');
    if (ANGLE[i] >= 0) Serial.print('+');
    if (ANGLE[i] > -10 && ANGLE[i] < 10) Serial.print(' ');
    Serial.print(ANGLE[i]); Serial.print(F("度 |"));

    if ((fault >> i) & 1) {              // 這顆失效
      Serial.println(F("XX 故障"));
      continue;
    }
    int8_t n = cm[i] / 5;                // 每 5 公分一格，最多 24 格
    if (n > 24) n = 24;
    for (int8_t k = 0; k < n; k++) Serial.print('#');
    if (cm[i] >= 120) Serial.println(F("> 沒有東西"));
    else { Serial.print(' '); Serial.print(cm[i]); Serial.println(F(" 公分")); }
  }
  delay(50);
  delay(450);                            // 每 0.5 秒重畫一次   👉 可以改
}

// ============================================================
// 以下是「通訊小幫手」（MBC I2C 協議 v2）：負責跟感應器講話。
// 所有 MBC 範例都一樣，不用改，也不用看懂。
//   讀法：寫 [編號] → 等 2 ms → 讀 2×k 個 byte = 從該編號起連續 k 個 int16（小端）
//   沒有這個編號（空號）時感應器回 -32768（MBC_NONE）
// ============================================================

// 開始 I2C 通訊（setup 裡呼叫一次）
// UNO/Nano：SDA=A4、SCL=A5；UNO R4：SDA/SCL 腳；ESP32：預設 GPIO21(SDA)/22(SCL)
void mbcBegin() {
  Wire.begin();
#if defined(WIRE_HAS_TIMEOUT)
  Wire.setWireTimeout(25000, true);      // 線沒接好時，不會讓 Arduino 當機卡住
#endif
}

// 從位址 addr 的感應器，由編號 id 起連續讀 k 個值（1~16）放進 out。全部讀到回傳 true
bool mbcReadMany(uint8_t addr, uint8_t id, uint8_t k, int16_t *out) {
  if (k < 1) k = 1;
  if (k > 16) k = 16;                    // 一次最多 16 個值（32 byte，Wire 緩衝區上限）
  Wire.beginTransmission(addr);
  Wire.write(id);                        // 告訴感應器：我要從第 id 號開始讀
  if (Wire.endTransmission() != 0) return false;
  delay(2);                              // 等 2 ms 再讀（ESP32 型的產品一定要等）
  uint8_t n = (uint8_t)(2 * k);
  if (Wire.requestFrom(addr, n) != n) return false;
  for (uint8_t i = 0; i < k; i++) {
    uint16_t lo = Wire.read();           // 先來的是低位元組
    uint16_t hi = Wire.read();           // 再來高位元組（小端）
    out[i] = (int16_t)(uint16_t)(lo | (hi << 8));
  }
  return true;
}

// 讀一個值。讀不到（沒接好）回傳 MBC_NONE
int16_t mbcRead(uint8_t addr, uint8_t id) {
  int16_t v;
  return mbcReadMany(addr, id, 1, &v) ? v : MBC_NONE;
}

// 送命令：寫 [id][參數…]。p 指向參數、n = 參數幾個 byte（沒參數就不填）。成功回傳 true
bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p, uint8_t n) {
  Wire.beginTransmission(addr);
  Wire.write(id);
  for (uint8_t i = 0; i < n; i++) Wire.write(p[i]);
  return Wire.endTransmission() == 0;
}

// 等感應器準備好（狀態旗標 F3 的 bit0 = 1），最多等 ms 毫秒。準備好了回傳 true
bool mbcWaitReady(uint8_t addr, uint16_t ms) {
  unsigned long start = millis();
  while (millis() - start < ms) {
    int16_t f = mbcRead(addr, 0xF3);
    if (f != MBC_NONE && (f & 1)) return true;
    delay(50);
  }
  return false;
}

你會看到：（正前方 30 公分有一本書）

====== 雷達畫面（長條越長 = 越空曠）======
1 -90度 |########################> 沒有東西
2 -60度 |########################> 沒有東西
3 -30度 |############### 75 公分
4 + 0度 |###### 32 公分
5 + 0度 |###### 34 公分
6 +30度 |################ 80 公分
7 +60度 |###################### 110 公分
8 +90度 |########################> 沒有東西

6️⃣ 避障小車（進階）

做什麼： 讓兩輪小車自己繞過障礙物：05 正前方 cm 還遠就直走；前面有東西就看 02 最近方向，東西在左往右轉、在右往左轉；09 正前方 mm 太近就先倒退。一次 mbcReadMany 從 01 讀到 09，所有判斷用同一瞬間的資料。

另外要準備： 兩輪小車、L298N 馬達驅動板（或同樣接法的 TB6612）、電池。

馬達接線：

第一次跑之前： 把車子架高（輪子懸空）再上傳，確認兩個輪子都是往前轉。哪個輪子轉反了，就把 LEFT_REVERSE 或 RIGHT_REVERSE 改成 true。

下載 tof8_06_avoid_car.ino

/*
 * 測距者 8 路 × Arduino　範例 6（進階）：避障小車
 * ------------------------------------------------------------
 * 這個範例會做什麼？
 *   讓兩輪小車自己繞過障礙物：
 *     - 「05 正前方 cm」還很遠 → 直走
 *     - 正前方有東西 → 看「02 最近方向」，東西在左就往右轉、在右就往左轉
 *     - 「09 正前方 mm」太近（快撞到）→ 先倒退一下再轉
 *   用 mbcReadMany 從 01 一次讀到 09，所有判斷都用同一瞬間的資料。
 *
 * 你需要：兩輪小車、L298N 馬達驅動板（或同樣接法的 TB6612 等）、電池。
 *
 * 接線：
 *   感應器 VCC → 5V      感應器 GND → GND
 *   感應器 SDA → A4      感應器 SCL → A5         （UNO / Nano）
 *   UNO R4 接 SDA / SCL 腳；ESP32 接 GPIO 21 (SDA) / GPIO 22 (SCL)
 *   L298N ENA → 5     IN1 → 7     IN2 → 8      （左輪）
 *   L298N ENB → 6     IN3 → 9     IN4 → 10     （右輪）
 *   （ESP32 改接：ENA → 25、IN1 → 26、IN2 → 27、ENB → 14、IN3 → 32、IN4 → 33）
 *   L298N 的 GND 要跟 Arduino 的 GND 接在一起！感應器裝在車頭正中間，雷射朝前。
 *
 * 第一次跑之前：
 *   把車子架高（輪子懸空）上傳，確認兩個輪子都是「往前」轉。
 *   哪個輪子轉反了，就把 LEFT_REVERSE 或 RIGHT_REVERSE 改成 true。
 */

#include <Wire.h>

const uint8_t ADDR = 0x29;               // 測距者的門牌號碼（I2C 位址）
const int16_t MBC_NONE = -32768;         // 「沒有這個值／讀不到」的代號

bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p = NULL, uint8_t n = 0);

// ---- 馬達接腳 ----
#if defined(ESP32)
const int ENA = 25, IN1 = 26, IN2 = 27;  // 左輪（ESP32 的 6~11 腳不能用）
const int ENB = 14, IN3 = 32, IN4 = 33;  // 右輪
#else
const int ENA = 5, IN1 = 7, IN2 = 8;     // 左輪
const int ENB = 6, IN3 = 9, IN4 = 10;    // 右輪
#endif

// ---- 👉 可以改的參數 ----
const int     SPEED      = 130;          // 直走速度 0~255
const int     TURN_SPEED = 110;          // 轉彎速度
const int16_t TURN_CM    = 35;           // 正前方幾公分以內要轉彎
const int16_t BACK_MM    = 120;          // 正前方幾 mm 以內先倒退
const bool    LEFT_REVERSE  = false;     // 左輪轉反了改成 true
const bool    RIGHT_REVERSE = false;     // 右輪轉反了改成 true

void setup() {
  Serial.begin(115200);
  pinMode(ENA, OUTPUT); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  drive(0, 0);
  mbcBegin();
  delay(500);
  if (!mbcWaitReady(ADDR, 6000)) {
    Serial.println(F("找不到感應器，請先跑範例 1 檢查接線。"));
    while (true) {}
  }
  Serial.println(F("3 秒後出發！"));
  delay(3000);
}

void loop() {
  int16_t v[9];                          // 01~09
  if (!mbcReadMany(ADDR, 0x01, 9, v)) {  // 讀不到 → 安全停車
    drive(0, 0);
    delay(50);
    return;
  }
  int16_t nearCm  = v[0];                // 01 最近距離 cm
  int16_t nearDir = v[1];                // 02 最近方向（負 = 左）
  int16_t frontCm = v[4];                // 05 正前方 cm
  int16_t frontMm = v[8];                // 09 正前方 mm

  if (frontMm <= BACK_MM) {              // 快撞到了：先倒退
    Serial.println(F("太近！倒退"));
    drive(-TURN_SPEED, -TURN_SPEED);
    delay(300);
  } else if (frontCm <= TURN_CM || (nearCm < 20 && nearDir > -3 && nearDir < 3)) {
    // 正前方有東西（或很近的東西在前方附近）→ 往東西的反方向轉
    if (nearDir < 0) { Serial.println(F("障礙在左 → 右轉")); drive(TURN_SPEED, -TURN_SPEED); }
    else             { Serial.println(F("障礙在右 → 左轉")); drive(-TURN_SPEED, TURN_SPEED); }
    delay(200);
  } else {
    drive(SPEED, SPEED);                 // 前面空的：直走
  }
  delay(50);
}

void drive(int left, int right) {
  if (LEFT_REVERSE)  left  = -left;
  if (RIGHT_REVERSE) right = -right;
  setMotor(ENA, IN1, IN2, left);
  setMotor(ENB, IN3, IN4, right);
}

void setMotor(int en, int inA, int inB, int speed) {
  speed = constrain(speed, -255, 255);
  digitalWrite(inA, speed >= 0 ? HIGH : LOW);
  digitalWrite(inB, speed >= 0 ? LOW : HIGH);
  analogWrite(en, abs(speed));
}

// ============================================================
// 以下是「通訊小幫手」（MBC I2C 協議 v2）：負責跟感應器講話。
// 所有 MBC 範例都一樣，不用改，也不用看懂。
//   讀法：寫 [編號] → 等 2 ms → 讀 2×k 個 byte = 從該編號起連續 k 個 int16（小端）
//   沒有這個編號（空號）時感應器回 -32768（MBC_NONE）
// ============================================================

// 開始 I2C 通訊（setup 裡呼叫一次）
// UNO/Nano：SDA=A4、SCL=A5；UNO R4：SDA/SCL 腳；ESP32：預設 GPIO21(SDA)/22(SCL)
void mbcBegin() {
  Wire.begin();
#if defined(WIRE_HAS_TIMEOUT)
  Wire.setWireTimeout(25000, true);      // 線沒接好時，不會讓 Arduino 當機卡住
#endif
}

// 從位址 addr 的感應器，由編號 id 起連續讀 k 個值（1~16）放進 out。全部讀到回傳 true
bool mbcReadMany(uint8_t addr, uint8_t id, uint8_t k, int16_t *out) {
  if (k < 1) k = 1;
  if (k > 16) k = 16;                    // 一次最多 16 個值（32 byte，Wire 緩衝區上限）
  Wire.beginTransmission(addr);
  Wire.write(id);                        // 告訴感應器：我要從第 id 號開始讀
  if (Wire.endTransmission() != 0) return false;
  delay(2);                              // 等 2 ms 再讀（ESP32 型的產品一定要等）
  uint8_t n = (uint8_t)(2 * k);
  if (Wire.requestFrom(addr, n) != n) return false;
  for (uint8_t i = 0; i < k; i++) {
    uint16_t lo = Wire.read();           // 先來的是低位元組
    uint16_t hi = Wire.read();           // 再來高位元組（小端）
    out[i] = (int16_t)(uint16_t)(lo | (hi << 8));
  }
  return true;
}

// 讀一個值。讀不到（沒接好）回傳 MBC_NONE
int16_t mbcRead(uint8_t addr, uint8_t id) {
  int16_t v;
  return mbcReadMany(addr, id, 1, &v) ? v : MBC_NONE;
}

// 送命令：寫 [id][參數…]。p 指向參數、n = 參數幾個 byte（沒參數就不填）。成功回傳 true
bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p, uint8_t n) {
  Wire.beginTransmission(addr);
  Wire.write(id);
  for (uint8_t i = 0; i < n; i++) Wire.write(p[i]);
  return Wire.endTransmission() == 0;
}

// 等感應器準備好（狀態旗標 F3 的 bit0 = 1），最多等 ms 毫秒。準備好了回傳 true
bool mbcWaitReady(uint8_t addr, uint16_t ms) {
  unsigned long start = millis();
  while (millis() - start < ms) {
    int16_t f = mbcRead(addr, 0xF3);
    if (f != MBC_NONE && (f & 1)) return true;
    delay(50);
  }
  return false;
}

怎麼調整：

7️⃣ 測距模式：一般／遠距離／精準／高速（E0）

做什麼： 用命令 E0 切換 4 種測距模式，看最遠能量多遠、數字多穩。序列埠監控窗下方輸入 0～3 按 Enter 換模式，每次換完都讀回 20～23 印出目前設定。

另外要準備： 一面白牆或大白板，放在正前方（第 4、5 顆雷射的方向）約 150 公分。

步驟：

1. 照上面的表格接好 4 條線，USB 插上電腦。
2. 複製下面的程式，貼到 Arduino IDE，按 → 上傳。
3. 打開序列埠監控窗，速度選 115200，下方輸入框的換行選「NL」或「Both NL & CR」都可以。

下載 tof8_07_range_mode.ino

/*
 * 測距者 8 路（命令範例 01）× Arduino：測距模式：一般／遠距離／精準／高速
 * ------------------------------------------------------------
 * 這個範例會做什麼？
 *   用命令 E0 切換 4 種測距模式，看最遠能量多遠、數字多穩：
 *     0 一般（出廠，最遠 120 cm、每 33 ms 量一次）
 *     1 遠距離（最遠 200 cm，比較容易被陽光、黑色東西影響）
 *     2 精準（每 100 ms 量一次，數字最穩，最遠 120 cm）
 *     3 高速（每 20 ms 量一次，反應最快，數字會跳比較多）
 *
 * 你需要：測距者 8 路 通用 I2C 版 ＋ 一面白牆或大白板，放在正前方（第 4、5 顆雷射的方向）約 150 公分。
 *
 * 接線：
 *   感應器 VCC → 5V      感應器 GND → GND
 *   感應器 SDA → A4      感應器 SCL → A5         （UNO / Nano）
 *   UNO R4 接 SDA / SCL 腳；ESP32 接 GPIO 21 (SDA) / GPIO 22 (SCL)
 *
 * 怎麼看結果：
 *   序列埠監控窗 115200，下方輸入框打 0～3 再按 Enter 換模式。
 *   一般模式量 150 cm 的牆會讀到 120（＝超過上限、當成沒東西）；換成 1 遠距離就讀得到約 150。
 *
 * 小提醒：
 *   換模式時，更新時間和距離上限會一起回到那個模式的預設值（要搭配 E2、E4 就先送 E0）。
 *   設定只存在感應器的記憶體，斷電就回到一般模式。
 *   命令表：E0 測距模式、E1 濾波、E2 更新時間、E3 回出廠、E4 距離上限（每個都帶 1 個 byte 參數）。
 *   讀回目前設定：20 測距模式、21 濾波、22 更新時間 ms、23 距離上限 cm。
 */

#include <Wire.h>

const uint8_t ADDR = 0x29;               // 測距者的門牌號碼（I2C 位址）
const int16_t MBC_NONE = -32768;         // 「沒有這個值／讀不到」的代號

bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p = NULL, uint8_t n = 0);
bool mbcCommand(uint8_t cmd, uint8_t arg);
void printSettings();

void setup() {
  Serial.begin(115200);
  Serial.setTimeout(50);
  mbcBegin();
  delay(500);
  if (!mbcWaitReady(ADDR, 4000)) {
    Serial.println(F("找不到感應器，請先跑範例 1 檢查接線。"));
    while (true) {}
  }
  mbcCommand(0xE3, 0xA5);                // 先回出廠設定，從乾淨的狀態開始
  Serial.println(F("輸入 0～3 換測距模式（0 一般、1 遠距離、2 精準、3 高速）"));
  printSettings();
}

void loop() {
  if (Serial.available()) {
    int m = Serial.parseInt();
    while (Serial.available()) Serial.read();
    if (m >= 0 && m <= 3) {
      Serial.print(F(">>> 送 E0 ")); Serial.println(m);
      if (!mbcCommand(0xE0, m)) Serial.println(F("命令沒做完（逾時）"));
      printSettings();
    }
  }
  int16_t d[9];  // 01 最近 cm、02 最近方向、03 最空曠 cm、04 最空曠方向、05 正前方 cm、06 最近 mm、07 初始化、08 故障、09 正前方 mm
  if (mbcReadMany(ADDR, 0x01, 9, d)) {
    Serial.print(F("正前方 ")); Serial.print(d[4]); Serial.print(F(" cm（")); Serial.print(d[8]);
    Serial.print(F(" mm）、最近 ")); Serial.print(d[0]); Serial.print(F(" cm 在方向 ")); Serial.print(d[1]);
    Serial.println();
  }
  delay(300);
}

// 送一個命令 [命令碼][參數]，然後等感應器做完（狀態旗標 F3 的 bit2「忙碌」變 0），最多等 3 秒
// 測距模式、更新時間、回出廠要重新設定雷射晶片，約 0.1～0.5 秒；濾波、距離上限馬上生效
bool mbcCommand(uint8_t cmd, uint8_t arg) {
  if (!mbcWrite(ADDR, cmd, &arg, 1)) return false;
  delay(20);                             // 讓感應器把命令排進去
  unsigned long start = millis();
  while (millis() - start < 3000) {
    int16_t f = mbcRead(ADDR, 0xF3);
    if (f != MBC_NONE && !(f & 4)) return true;   // bit2 = 0：做完了
    delay(10);
  }
  return false;
}

// 從感應器讀回目前的 4 個設定並印出來（20 測距模式、21 濾波、22 更新時間 ms、23 距離上限 cm）
void printSettings() {
  int16_t s[4];
  if (!mbcReadMany(ADDR, 0x20, 4, s)) { Serial.println(F("讀不到設定")); return; }
  const char *modeName[] = {"一般", "遠距離", "精準", "高速"};
  Serial.print(F("【目前設定】測距模式 ")); Serial.print(s[0]); Serial.print(F(" "));
  Serial.print((s[0] >= 0 && s[0] <= 3) ? modeName[s[0]] : "?");
  Serial.print(F("｜濾波 ")); Serial.print(s[1]);
  Serial.print(F("｜更新時間 ")); Serial.print(s[2]); Serial.print(F(" ms"));
  Serial.print(F("｜距離上限 ")); Serial.print(s[3]); Serial.println(F(" cm"));
}

// ============================================================
// 以下是「通訊小幫手」（MBC I2C 協議 v2）：負責跟感應器講話。
// 所有 MBC 範例都一樣，不用改，也不用看懂。
//   讀法：寫 [編號] → 等 2 ms → 讀 2×k 個 byte = 從該編號起連續 k 個 int16（小端）
//   沒有這個編號（空號）時感應器回 -32768（MBC_NONE）
// ============================================================

// 開始 I2C 通訊（setup 裡呼叫一次）
// UNO/Nano：SDA=A4、SCL=A5；UNO R4：SDA/SCL 腳；ESP32：預設 GPIO21(SDA)/22(SCL)
void mbcBegin() {
  Wire.begin();
#if defined(WIRE_HAS_TIMEOUT)
  Wire.setWireTimeout(25000, true);      // 線沒接好時，不會讓 Arduino 當機卡住
#endif
}

// 從位址 addr 的感應器，由編號 id 起連續讀 k 個值（1~16）放進 out。全部讀到回傳 true
bool mbcReadMany(uint8_t addr, uint8_t id, uint8_t k, int16_t *out) {
  if (k < 1) k = 1;
  if (k > 16) k = 16;                    // 一次最多 16 個值（32 byte，Wire 緩衝區上限）
  Wire.beginTransmission(addr);
  Wire.write(id);                        // 告訴感應器：我要從第 id 號開始讀
  if (Wire.endTransmission() != 0) return false;
  delay(2);                              // 等 2 ms 再讀（ESP32 型的產品一定要等）
  uint8_t n = (uint8_t)(2 * k);
  if (Wire.requestFrom(addr, n) != n) return false;
  for (uint8_t i = 0; i < k; i++) {
    uint16_t lo = Wire.read();           // 先來的是低位元組
    uint16_t hi = Wire.read();           // 再來高位元組（小端）
    out[i] = (int16_t)(uint16_t)(lo | (hi << 8));
  }
  return true;
}

// 讀一個值。讀不到（沒接好）回傳 MBC_NONE
int16_t mbcRead(uint8_t addr, uint8_t id) {
  int16_t v;
  return mbcReadMany(addr, id, 1, &v) ? v : MBC_NONE;
}

// 送命令：寫 [id][參數…]。p 指向參數、n = 參數幾個 byte（沒參數就不填）。成功回傳 true
bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p, uint8_t n) {
  Wire.beginTransmission(addr);
  Wire.write(id);
  for (uint8_t i = 0; i < n; i++) Wire.write(p[i]);
  return Wire.endTransmission() == 0;
}

// 等感應器準備好（狀態旗標 F3 的 bit0 = 1），最多等 ms 毫秒。準備好了回傳 true
bool mbcWaitReady(uint8_t addr, uint16_t ms) {
  unsigned long start = millis();
  while (millis() - start < ms) {
    int16_t f = mbcRead(addr, 0xF3);
    if (f != MBC_NONE && (f & 1)) return true;
    delay(50);
  }
  return false;
}

你會看到：

【目前設定】測距模式 0 一般｜濾波 1｜更新時間 33 ms｜距離上限 120 cm
正前方 120 cm（1200 mm）、最近 120 cm 在方向 8
>>> 送 E0 1
【目前設定】測距模式 1 遠距離｜濾波 1｜更新時間 33 ms｜距離上限 200 cm
正前方 150 cm（1503 mm）、最近 150 cm 在方向 0

試試看： 一般模式量 150 公分的牆讀到 120（超過上限＝當成沒東西）；換成 1 遠距離就讀得到。再試 2 精準、3 高速，看數字跳動有什麼不同。遠距離模式比較容易被陽光、黑色的東西影響。

8️⃣ 濾波：讓數字比較穩（E1）

做什麼： 用命令 E1 改濾波，看同一個東西擺著不動時數字跳多少。輸入 0～2 按 Enter 換濾波；每秒印「1 秒內最大－最小」＝跳動幾 mm。

另外要準備： 一個盒子或白板，放在正前方約 50 公分，擺好不要動。

步驟：

1. 照上面的表格接好 4 條線，USB 插上電腦。
2. 複製下面的程式，貼到 Arduino IDE，按 → 上傳。
3. 打開序列埠監控窗，速度選 115200，下方輸入框的換行選「NL」或「Both NL & CR」都可以。

下載 tof8_08_filter.ino

/*
 * 測距者 8 路（命令範例 02）× Arduino：濾波：讓數字比較穩
 * ------------------------------------------------------------
 * 這個範例會做什麼？
 *   用命令 E1 改濾波，看同一個東西擺著不動時，數字跳動多少：
 *     0 關（每次量到多少就是多少，反應最快、跳最多）
 *     1 最近 3 筆取中位數（出廠）
 *     2 最近 5 筆取中位數（最穩，反應慢一點）
 *
 * 你需要：測距者 8 路 通用 I2C 版 ＋ 一個盒子或白板，放在正前方（第 4、5 顆雷射的方向）約 50 公分，擺好不要動。
 *
 * 接線：
 *   感應器 VCC → 5V      感應器 GND → GND
 *   感應器 SDA → A4      感應器 SCL → A5         （UNO / Nano）
 *   UNO R4 接 SDA / SCL 腳；ESP32 接 GPIO 21 (SDA) / GPIO 22 (SCL)
 *
 * 怎麼看結果：
 *   序列埠監控窗 115200，輸入 0～2 按 Enter 換濾波。每秒印一次「1 秒內最大−最小」＝跳動幾 mm。
 *
 * 小提醒：
 *   中位數會把偶爾一次的怪值濾掉，但東西移動時數字會慢 1～2 次才跟上。比賽要快就用 0，要穩就用 2。
 *   命令表：E0 測距模式、E1 濾波、E2 更新時間、E3 回出廠、E4 距離上限（每個都帶 1 個 byte 參數）。
 *   讀回目前設定：20 測距模式、21 濾波、22 更新時間 ms、23 距離上限 cm。
 */

#include <Wire.h>

const uint8_t ADDR = 0x29;               // 測距者的門牌號碼（I2C 位址）
const int16_t MBC_NONE = -32768;         // 「沒有這個值／讀不到」的代號

bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p = NULL, uint8_t n = 0);
bool mbcCommand(uint8_t cmd, uint8_t arg);
void printSettings();

void setup() {
  Serial.begin(115200);
  Serial.setTimeout(50);
  mbcBegin();
  delay(500);
  if (!mbcWaitReady(ADDR, 4000)) {
    Serial.println(F("找不到感應器，請先跑範例 1 檢查接線。"));
    while (true) {}
  }
  mbcCommand(0xE3, 0xA5);                // 先回出廠設定，從乾淨的狀態開始
  Serial.println(F("輸入 0～2 換濾波（0 關、1 三筆中位數、2 五筆中位數）"));
  printSettings();
}

void loop() {
  if (Serial.available()) {
    int f = Serial.parseInt();
    while (Serial.available()) Serial.read();
    if (f >= 0 && f <= 2) {
      Serial.print(F(">>> 送 E1 ")); Serial.println(f);
      mbcCommand(0xE1, f);
      printSettings();
    }
  }
  // 1 秒內一直讀 正前方 mm，記下最大、最小
  int16_t lo = 32767, hi = -32768;
  unsigned long start = millis();
  while (millis() - start < 1000) {
    int16_t d[9];  // 01 最近 cm、02 最近方向、03 最空曠 cm、04 最空曠方向、05 正前方 cm、06 最近 mm、07 初始化、08 故障、09 正前方 mm
    if (mbcReadMany(ADDR, 0x01, 9, d)) {
      if (d[8] < lo) lo = d[8];
      if (d[8] > hi) hi = d[8];
    }
    delay(20);
  }
  Serial.print(F("正前方 mm ")); Serial.print(lo); Serial.print(F("～")); Serial.print(hi);
  Serial.print(F("，跳動 ")); Serial.print(hi - lo); Serial.println(F(" mm"));
}

// 送一個命令 [命令碼][參數]，然後等感應器做完（狀態旗標 F3 的 bit2「忙碌」變 0），最多等 3 秒
// 測距模式、更新時間、回出廠要重新設定雷射晶片，約 0.1～0.5 秒；濾波、距離上限馬上生效
bool mbcCommand(uint8_t cmd, uint8_t arg) {
  if (!mbcWrite(ADDR, cmd, &arg, 1)) return false;
  delay(20);                             // 讓感應器把命令排進去
  unsigned long start = millis();
  while (millis() - start < 3000) {
    int16_t f = mbcRead(ADDR, 0xF3);
    if (f != MBC_NONE && !(f & 4)) return true;   // bit2 = 0：做完了
    delay(10);
  }
  return false;
}

// 從感應器讀回目前的 4 個設定並印出來（20 測距模式、21 濾波、22 更新時間 ms、23 距離上限 cm）
void printSettings() {
  int16_t s[4];
  if (!mbcReadMany(ADDR, 0x20, 4, s)) { Serial.println(F("讀不到設定")); return; }
  const char *modeName[] = {"一般", "遠距離", "精準", "高速"};
  Serial.print(F("【目前設定】測距模式 ")); Serial.print(s[0]); Serial.print(F(" "));
  Serial.print((s[0] >= 0 && s[0] <= 3) ? modeName[s[0]] : "?");
  Serial.print(F("｜濾波 ")); Serial.print(s[1]);
  Serial.print(F("｜更新時間 ")); Serial.print(s[2]); Serial.print(F(" ms"));
  Serial.print(F("｜距離上限 ")); Serial.print(s[3]); Serial.println(F(" cm"));
}

// ============================================================
// 以下是「通訊小幫手」（MBC I2C 協議 v2）：負責跟感應器講話。
// 所有 MBC 範例都一樣，不用改，也不用看懂。
//   讀法：寫 [編號] → 等 2 ms → 讀 2×k 個 byte = 從該編號起連續 k 個 int16（小端）
//   沒有這個編號（空號）時感應器回 -32768（MBC_NONE）
// ============================================================

// 開始 I2C 通訊（setup 裡呼叫一次）
// UNO/Nano：SDA=A4、SCL=A5；UNO R4：SDA/SCL 腳；ESP32：預設 GPIO21(SDA)/22(SCL)
void mbcBegin() {
  Wire.begin();
#if defined(WIRE_HAS_TIMEOUT)
  Wire.setWireTimeout(25000, true);      // 線沒接好時，不會讓 Arduino 當機卡住
#endif
}

// 從位址 addr 的感應器，由編號 id 起連續讀 k 個值（1~16）放進 out。全部讀到回傳 true
bool mbcReadMany(uint8_t addr, uint8_t id, uint8_t k, int16_t *out) {
  if (k < 1) k = 1;
  if (k > 16) k = 16;                    // 一次最多 16 個值（32 byte，Wire 緩衝區上限）
  Wire.beginTransmission(addr);
  Wire.write(id);                        // 告訴感應器：我要從第 id 號開始讀
  if (Wire.endTransmission() != 0) return false;
  delay(2);                              // 等 2 ms 再讀（ESP32 型的產品一定要等）
  uint8_t n = (uint8_t)(2 * k);
  if (Wire.requestFrom(addr, n) != n) return false;
  for (uint8_t i = 0; i < k; i++) {
    uint16_t lo = Wire.read();           // 先來的是低位元組
    uint16_t hi = Wire.read();           // 再來高位元組（小端）
    out[i] = (int16_t)(uint16_t)(lo | (hi << 8));
  }
  return true;
}

// 讀一個值。讀不到（沒接好）回傳 MBC_NONE
int16_t mbcRead(uint8_t addr, uint8_t id) {
  int16_t v;
  return mbcReadMany(addr, id, 1, &v) ? v : MBC_NONE;
}

// 送命令：寫 [id][參數…]。p 指向參數、n = 參數幾個 byte（沒參數就不填）。成功回傳 true
bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p, uint8_t n) {
  Wire.beginTransmission(addr);
  Wire.write(id);
  for (uint8_t i = 0; i < n; i++) Wire.write(p[i]);
  return Wire.endTransmission() == 0;
}

// 等感應器準備好（狀態旗標 F3 的 bit0 = 1），最多等 ms 毫秒。準備好了回傳 true
bool mbcWaitReady(uint8_t addr, uint16_t ms) {
  unsigned long start = millis();
  while (millis() - start < ms) {
    int16_t f = mbcRead(addr, 0xF3);
    if (f != MBC_NONE && (f & 1)) return true;
    delay(50);
  }
  return false;
}

你會看到：

【目前設定】測距模式 0 一般｜濾波 1｜更新時間 33 ms｜距離上限 120 cm
正前方 mm 498～506，跳動 8 mm
>>> 送 E1 0
正前方 mm 492～511，跳動 19 mm

試試看： 中位數會把偶爾一次的怪值濾掉，但東西移動時數字會慢 1～2 次才跟上。比賽要快就用 0，要穩就用 2。

9️⃣ 更新時間：快一點還是準一點（E2）

做什麼： 用命令 E2 改「每量一次花多少時間」（20～200 ms），看每秒更新幾次、數字跳多少。程式一開始先把濾波關掉（E1 0），比較看得出差別。輸入 20～200 按 Enter；0＝回到模式預設。

另外要準備： 一個盒子或白板，放在正前方約 50 公分，擺好不要動。

步驟：

1. 照上面的表格接好 4 條線，USB 插上電腦。
2. 複製下面的程式，貼到 Arduino IDE，按 → 上傳。
3. 打開序列埠監控窗，速度選 115200，下方輸入框的換行選「NL」或「Both NL & CR」都可以。

下載 tof8_09_timing_budget.ino

/*
 * 測距者 8 路（命令範例 03）× Arduino：更新時間：快一點還是準一點
 * ------------------------------------------------------------
 * 這個範例會做什麼？
 *   用命令 E2 改「每量一次花多少時間」（20～200 ms），看每秒更新幾次、數字跳多少：
 *     20 ms：一秒更新約 50 次，反應最快，數字跳比較多
 *     33 ms：出廠值
 *     200 ms：一秒更新 5 次，最穩、量得比較遠
 *
 * 你需要：測距者 8 路 通用 I2C 版 ＋ 一個盒子或白板，放在正前方（第 4、5 顆雷射的方向）約 50 公分，擺好不要動。
 *
 * 接線：
 *   感應器 VCC → 5V      感應器 GND → GND
 *   感應器 SDA → A4      感應器 SCL → A5         （UNO / Nano）
 *   UNO R4 接 SDA / SCL 腳；ESP32 接 GPIO 21 (SDA) / GPIO 22 (SCL)
 *
 * 怎麼看結果：
 *   序列埠監控窗 115200，輸入 20～200 按 Enter。每秒印「讀到的值換了幾次」和「跳動幾 mm」。
 *
 * 小提醒：
 *   超出 20～200 會被夾住（例：輸入 5 變 20、300 變 200）；輸入 0 = 回到目前模式的預設值。
 *   濾波（範例 2）會讓「換了幾次」變少一點，想看最原始的速度可以先把濾波設 0。
 *   命令表：E0 測距模式、E1 濾波、E2 更新時間、E3 回出廠、E4 距離上限（每個都帶 1 個 byte 參數）。
 *   讀回目前設定：20 測距模式、21 濾波、22 更新時間 ms、23 距離上限 cm。
 */

#include <Wire.h>

const uint8_t ADDR = 0x29;               // 測距者的門牌號碼（I2C 位址）
const int16_t MBC_NONE = -32768;         // 「沒有這個值／讀不到」的代號

bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p = NULL, uint8_t n = 0);
bool mbcCommand(uint8_t cmd, uint8_t arg);
void printSettings();

void setup() {
  Serial.begin(115200);
  Serial.setTimeout(50);
  mbcBegin();
  delay(500);
  if (!mbcWaitReady(ADDR, 4000)) {
    Serial.println(F("找不到感應器，請先跑範例 1 檢查接線。"));
    while (true) {}
  }
  mbcCommand(0xE3, 0xA5);                // 先回出廠設定，從乾淨的狀態開始
  mbcCommand(0xE1, 0);                   // 先關濾波，比較看得出差別
  Serial.println(F("輸入 20～200 換更新時間（ms），0 = 回預設"));
  printSettings();
}

void loop() {
  if (Serial.available()) {
    long b = Serial.parseInt();
    while (Serial.available()) Serial.read();
    if (b >= 0 && b <= 255) {
      Serial.print(F(">>> 送 E2 ")); Serial.println(b);
      if (!mbcCommand(0xE2, (uint8_t)b)) Serial.println(F("命令沒做完（逾時）"));
      printSettings();
    }
  }
  int16_t last = -1, lo = 32767, hi = -32768;
  int changes = 0;
  unsigned long start = millis();
  while (millis() - start < 1000) {      // 1 秒內一直讀：值換了幾次＝大約更新幾次
    int16_t d[9];  // 01 最近 cm、02 最近方向、03 最空曠 cm、04 最空曠方向、05 正前方 cm、06 最近 mm、07 初始化、08 故障、09 正前方 mm
    if (mbcReadMany(ADDR, 0x01, 9, d)) {
      if (d[8] != last) { changes++; last = d[8]; }
      if (d[8] < lo) lo = d[8];
      if (d[8] > hi) hi = d[8];
    }
    delay(5);
  }
  Serial.print(F("1 秒內換了 ")); Serial.print(changes); Serial.print(F(" 次，正前方 mm 跳動 "));
  Serial.print(hi - lo); Serial.println(F(" mm"));
}

// 送一個命令 [命令碼][參數]，然後等感應器做完（狀態旗標 F3 的 bit2「忙碌」變 0），最多等 3 秒
// 測距模式、更新時間、回出廠要重新設定雷射晶片，約 0.1～0.5 秒；濾波、距離上限馬上生效
bool mbcCommand(uint8_t cmd, uint8_t arg) {
  if (!mbcWrite(ADDR, cmd, &arg, 1)) return false;
  delay(20);                             // 讓感應器把命令排進去
  unsigned long start = millis();
  while (millis() - start < 3000) {
    int16_t f = mbcRead(ADDR, 0xF3);
    if (f != MBC_NONE && !(f & 4)) return true;   // bit2 = 0：做完了
    delay(10);
  }
  return false;
}

// 從感應器讀回目前的 4 個設定並印出來（20 測距模式、21 濾波、22 更新時間 ms、23 距離上限 cm）
void printSettings() {
  int16_t s[4];
  if (!mbcReadMany(ADDR, 0x20, 4, s)) { Serial.println(F("讀不到設定")); return; }
  const char *modeName[] = {"一般", "遠距離", "精準", "高速"};
  Serial.print(F("【目前設定】測距模式 ")); Serial.print(s[0]); Serial.print(F(" "));
  Serial.print((s[0] >= 0 && s[0] <= 3) ? modeName[s[0]] : "?");
  Serial.print(F("｜濾波 ")); Serial.print(s[1]);
  Serial.print(F("｜更新時間 ")); Serial.print(s[2]); Serial.print(F(" ms"));
  Serial.print(F("｜距離上限 ")); Serial.print(s[3]); Serial.println(F(" cm"));
}

// ============================================================
// 以下是「通訊小幫手」（MBC I2C 協議 v2）：負責跟感應器講話。
// 所有 MBC 範例都一樣，不用改，也不用看懂。
//   讀法：寫 [編號] → 等 2 ms → 讀 2×k 個 byte = 從該編號起連續 k 個 int16（小端）
//   沒有這個編號（空號）時感應器回 -32768（MBC_NONE）
// ============================================================

// 開始 I2C 通訊（setup 裡呼叫一次）
// UNO/Nano：SDA=A4、SCL=A5；UNO R4：SDA/SCL 腳；ESP32：預設 GPIO21(SDA)/22(SCL)
void mbcBegin() {
  Wire.begin();
#if defined(WIRE_HAS_TIMEOUT)
  Wire.setWireTimeout(25000, true);      // 線沒接好時，不會讓 Arduino 當機卡住
#endif
}

// 從位址 addr 的感應器，由編號 id 起連續讀 k 個值（1~16）放進 out。全部讀到回傳 true
bool mbcReadMany(uint8_t addr, uint8_t id, uint8_t k, int16_t *out) {
  if (k < 1) k = 1;
  if (k > 16) k = 16;                    // 一次最多 16 個值（32 byte，Wire 緩衝區上限）
  Wire.beginTransmission(addr);
  Wire.write(id);                        // 告訴感應器：我要從第 id 號開始讀
  if (Wire.endTransmission() != 0) return false;
  delay(2);                              // 等 2 ms 再讀（ESP32 型的產品一定要等）
  uint8_t n = (uint8_t)(2 * k);
  if (Wire.requestFrom(addr, n) != n) return false;
  for (uint8_t i = 0; i < k; i++) {
    uint16_t lo = Wire.read();           // 先來的是低位元組
    uint16_t hi = Wire.read();           // 再來高位元組（小端）
    out[i] = (int16_t)(uint16_t)(lo | (hi << 8));
  }
  return true;
}

// 讀一個值。讀不到（沒接好）回傳 MBC_NONE
int16_t mbcRead(uint8_t addr, uint8_t id) {
  int16_t v;
  return mbcReadMany(addr, id, 1, &v) ? v : MBC_NONE;
}

// 送命令：寫 [id][參數…]。p 指向參數、n = 參數幾個 byte（沒參數就不填）。成功回傳 true
bool mbcWrite(uint8_t addr, uint8_t id, const uint8_t *p, uint8_t n) {
  Wire.beginTransmission(addr);
  Wire.write(id);
  for (uint8_t i = 0; i < n; i++) Wire.write(p[i]);
  return Wire.endTransmission() == 0;
}

// 等感應器準備好（狀態旗標 F3 的 bit0 = 1），最多等 ms 毫秒。準備好了回傳 true
bool mbcWaitReady(uint8_t addr, uint16_t ms) {
  unsigned long start = millis();
  while (millis() - start < ms) {
    int16_t f = mbcRead(addr, 0xF3);
    if (f != MBC_NONE && (f & 1)) return true;
    delay(50);
  }
  return false;
}