Ini
[platformio]
default_envs = esp32-wroom-32e

[env:esp32-wroom-32e]
platform = espressif32
board = esp32dev
framework = arduino
monitor_speed = 115200
upload_speed = 460800
monitor_filters = esp32_exception_decoder, time
lib_deps =
  pololu/VL53L1X@^1.3.1
  sparkfun/SparkFun LSM6DS3 Breakout@^1.0.3
build_flags =
  -DCORE_DEBUG_LEVEL=1
  -DARDUINO_USB_CDC_ON_BOOT=0

Cpp
#include <Arduino.h>
#include <Wire.h>
#include <WiFi.h>
#include <esp_bt.h>
#include <VL53L1X.h>
#include <SparkFunLSM6DS3.h>

namespace Pins {
constexpr uint8_t SDA = 21;
constexpr uint8_t SCL = 22;
constexpr uint8_t TOF_INT = 19;
constexpr uint8_t XSHUT[4] = {12, 13, 14, 15};
constexpr uint8_t IMU_INT = 18;
constexpr uint8_t RS485_RX = 16;
constexpr uint8_t RS485_TX = 17;
constexpr uint8_t RS485_DIR = 4;
}

constexpr uint32_t DEBUG_BAUD = 115200;
constexpr uint32_t RS485_BAUD = 115200;
constexpr uint8_t TOF_ADDR[4] = {0x30, 0x31, 0x32, 0x33};
constexpr uint8_t LSM6DS3_ADDR = 0x6B; // Change to 0x6A if the breakout's SA0 selects it.

VL53L1X tof[4];
LSM6DS3 imu(I2C_MODE, LSM6DS3_ADDR);
bool tofReady[4] = {false, false, false, false};
bool imuReady = false;
volatile uint32_t tofIrqCount = 0;
volatile uint32_t imuIrqCount = 0;

void IRAM_ATTR onTofInterrupt() { ++tofIrqCount; }
void IRAM_ATTR onImuInterrupt() { ++imuIrqCount; }

static void holdAllTofInReset() {
  // GPIO12 first: minimize time it could be influenced after startup.
  for (uint8_t i = 0; i < 4; ++i) {
    pinMode(Pins::XSHUT[i], OUTPUT);
    digitalWrite(Pins::XSHUT[i], LOW);
  }
  delay(10);
}

static bool initTofSensors() {
  holdAllTofInReset();
  bool allOk = true;

  for (uint8_t i = 0; i < 4; ++i) {
    digitalWrite(Pins::XSHUT[i], HIGH);
    delay(10);

    tof[i].setTimeout(250);
    if (!tof[i].init()) {
      Serial.printf("ERROR: VL53L1X[%u] did not initialize at default 0x29\n", i + 1);
      tofReady[i] = false;
      allOk = false;
      // Keep this channel released so a wiring fault is visible during an I2C scan.
      continue;
    }

    tof[i].setAddress(TOF_ADDR[i]);
    tof[i].setDistanceMode(VL53L1X::Long);
    tof[i].setMeasurementTimingBudget(50000);
    tof[i].startContinuous(100);
    tofReady[i] = true;
    Serial.printf("VL53L1X[%u] ready at 0x%02X\n", i + 1, TOF_ADDR[i]);
  }
  return allOk;
}

static void scanI2c() {
  Serial.println("I2C scan:");
  uint8_t count = 0;
  for (uint8_t address = 1; address < 127; ++address) {
    Wire.beginTransmission(address);
    if (Wire.endTransmission() == 0) {
      Serial.printf("  found 0x%02X\n", address);
      ++count;
    }
  }
  Serial.printf("I2C devices: %u\n", count);
}

static bool initImu() {
  const int status = imu.begin();
  if (status != 0) {
    Serial.printf("ERROR: LSM6DS3 begin failed: %d (check 0x%02X/0x6A)\n", status,
                  LSM6DS3_ADDR);
    return false;
  }
  Serial.printf("LSM6DS3 ready at 0x%02X\n", LSM6DS3_ADDR);
  // The interrupt pin is attached and counted. Configure the breakout's desired
  // data-ready/motion interrupt registers for the exact module variant if needed.
  return true;
}

static void rs485SetTransmit(bool enable) {
  digitalWrite(Pins::RS485_DIR, enable ? HIGH : LOW);
  if (enable) delayMicroseconds(20);
}

static void rs485SendLine(const String &line) {
  // PROVISIONAL ASCII frame, not Modbus and not the unspecified VEX protocol.
  // Format: $LOC,<millis>,<d1>,<d2>,<d3>,<d4>,<ax>,<ay>,<az>,<gx>,<gy>,<gz>*<xor>\r\n
  uint8_t checksum = 0;
  for (size_t i = 0; i < line.length(); ++i) checksum ^= uint8_t(line[i]);
  rs485SetTransmit(true);
  Serial2.print('

## Build, upload, and monitor

1. Install VS Code, PlatformIO, and a recent CP210x driver if the OS does not provide one.
2. Create the files exactly as shown (`platformio.ini`, `src/main.cpp`).
3. Connect USB-C with a known-good data cable. During first bring-up, leave 12 V and external sensors disconnected.
4. Build:

```bash
pio run

5. Upload using CP2102N auto-reset:

Bash
pio run -t upload

6. Monitor UART0:

Bash
pio device monitor -b 115200

Text
VEX localization board firmware starter
WiFi/Bluetooth disabled
VL53L1X[1] ready at 0x30
VL53L1X[2] ready at 0x31
VL53L1X[3] ready at 0x32
VL53L1X[4] ready at 0x33
LSM6DS3 ready at 0x6B
I2C scan:
  found 0x30
  found 0x31
  found 0x32
  found 0x33
  found 0x6B
I2C devices: 5
Expected ToF addresses: 0x30 0x31 0x32 0x33
Setup complete
d=[...,...,...,...]mm a=[...,...,...]g irq[tof=... imu=...]

• J3-J6 carry VL53L1X breakouts that accept 3.3 V power and logic and expose active-low XSHUT.
• The Pololu VL53L1X and SparkFun LSM6DS3 Arduino libraries are used.
• The LSM6DS3 starts at 0x6B unless its breakout strap selects 0x6A.
• J8 is a logic-level breakout; the external half-duplex RS485 transceiver must be 3.3 V compatible and interpret GPIO4 high as transmit-enable.
• UART2 uses 115200 8N1. The $LOC,...*XX XOR-framed telemetry is provisional, not Modbus, and not claimed compatible with any VEX application protocol.
• WiFi and Bluetooth remain disabled by default to reduce current during bring-up.); Serial2.print(line); Serial2.printf("*%02X\r\n", checksum); Serial2.flush(); delayMicroseconds((1000000UL * 10UL) / RS485_BAUD); // one character time rs485SetTransmit(false); }

## Build, upload, and monitor

1. Install VS Code, PlatformIO, and a recent CP210x driver if the OS does not provide one.
2. Create the files exactly as shown (`platformio.ini`, `src/main.cpp`).
3. Connect USB-C with a known-good data cable. During first bring-up, leave 12 V and external sensors disconnected.
4. Build:

FLUX_FENCED_CODE_BLOCK_2_PLACEHOLDER

5. Upload using CP2102N auto-reset:

FLUX_FENCED_CODE_BLOCK_3_PLACEHOLDER

6. Monitor UART0:

FLUX_FENCED_CODE_BLOCK_4_PLACEHOLDER

To specify a port, add `upload_port = COMx` / `/dev/ttyUSB0` and `monitor_port = ...` to the environment, or use `pio run -t upload --upload-port `.

### Manual bootloader fallback

If auto-reset does not enter the ROM loader: hold BOOT (SW2 / GPIO0), tap RESET (SW1 / EN), release RESET, start upload, then release BOOT when connection begins. Tap RESET after upload if the application does not start. Normal idle measurements are EN >2.8 V and IO0 >2.8 V; pressed level should be <0.4 V.

## Expected debug output

A healthy boot resembles:

FLUX_FENCED_CODE_BLOCK_5_PLACEHOLDER

If the IMU breakout uses address 0x6A, change `LSM6DS3_ADDR`. The common ToF interrupt line is shared, so firmware must poll sensors to identify the source. Exact LSM6DS3 interrupt register programming is breakout/configuration dependent; the starter attaches GPIO18 and verifies edge counting, while normal IMU reads are fully functional.

## Assumptions and protocol status

- J3-J6 carry VL53L1X breakouts that accept 3.3 V power and logic and expose active-low XSHUT.
- The Pololu VL53L1X and SparkFun LSM6DS3 Arduino libraries are used.
- The LSM6DS3 starts at 0x6B unless its breakout strap selects 0x6A.
- J8 is a logic-level breakout; the external half-duplex RS485 transceiver must be 3.3 V compatible and interpret GPIO4 high as transmit-enable.
- UART2 uses 115200 8N1. The `$LOC,...*XX` XOR-framed telemetry is provisional, not Modbus, and not claimed compatible with any VEX application protocol.
- WiFi and Bluetooth remain disabled by default to reduce current during bring-up.