Practical case: Self-Balancing Robot on Arduino Nano 33 BLE

Objective and use case

What you’ll build: You will create an IMU self-balancing rover utilizing the Arduino Nano 33 BLE Sense, TB6612FNG, and AS5600. This project involves fusing IMU data with wheel angle and speed to maintain balance.

Why it matters / Use cases

• Demonstrates real-time sensor fusion by integrating data from the LSM9DS1 IMU and AS5600 encoder for precise control.
• Provides a practical application of PID control theory in robotics, enhancing understanding of feedback systems.
• Showcases the capabilities of the Arduino Nano 33 BLE Sense in handling complex tasks beyond basic microcontroller functions.
• Offers insights into dual H-bridge motor control using the TB6612FNG for efficient power management in mobile robotics.

Expected outcome

• Achieve stable balancing with a maximum tilt angle of less than 5 degrees during operation.
• Maintain a response time of less than 100 ms for sensor data processing and motor adjustments.
• Demonstrate the ability to drive the rover at speeds up to 1 m/s while maintaining balance.
• Record successful operation over a continuous runtime of at least 30 minutes on a single battery charge.

Audience: Advanced embedded systems developers; Level: Advanced

Architecture/flow: The system architecture involves data acquisition from the LSM9DS1 IMU and AS5600, processing through the Arduino Nano 33 BLE Sense, and motor control via the TB6612FNG.

Advanced Hands‑on: IMU Self‑Balancing Rover with Arduino Nano 33 BLE Sense + TB6612FNG + AS5600

Objective: Build, program, and validate an imu-self-balancing-rover using the exact device model Arduino Nano 33 BLE Sense + TB6612FNG + AS5600. You will fuse IMU data (from the Nano 33 BLE Sense’s LSM9DS1) with wheel angle/speed (from the I2C AS5600 magnetic encoder), and drive a two‑wheel platform via the TB6612FNG dual H‑bridge. This guide uses PlatformIO (CLI) for a reproducible, scriptable build and upload process.

Note: The Arduino Nano 33 BLE Sense is not an AVR/UNO board; it uses an nRF52840 (mbed) core and native USB CDC. Per the family defaults, that means we use PlatformIO, not the Arduino CLI, and include driver notes. All commands shown are shell‑friendly and avoid the GUI.

---

Prerequisites

• Skills and knowledge:
• Advanced embedded C/C++ experience
• Basic control theory terminology (PID, complementary filter, derivative, saturation)
• Familiarity with PlatformIO and serial terminals
• Safe handling of Li‑ion/LiPo power systems
• Host OS:
• Windows 10/11, macOS 12+, or Ubuntu 22.04+
• Software:
• Python 3.10+ in PATH
• PlatformIO Core 6.1.11 or newer (we will show exact commands to install/update)
• Git (optional, for version control)
• Drivers/USB notes for Arduino Nano 33 BLE Sense:
• Uses native USB CDC (no CP210x/CH34x needed). On Windows 10+ and macOS, drivers are built‑in.
• On Windows 7/8, install Arduino LLC driver if required (legacy; not covered here). The board enumerates as a COM port.

Safety note: A balancing rover can rapidly move and fall. Test over a soft surface, keep hands clear of wheels, and have an emergency power cutoff.

---

Materials (exact model)

• 1x Arduino Nano 33 BLE Sense (ABX00031 or ABX00035, “BLE Sense Rev2” also supported)
• 1x TB6612FNG dual H‑bridge motor driver breakout (e.g., SparkFun ROB‑14450 or Pololu 713)
• 1x AS5600 I2C magnetic rotary encoder breakout (3.3V variant), with matching diametric magnet (e.g., 6 mm)
• 2x 6–12 V DC gearmotors (low backlash, similar KV, metal gearbox recommended)
• 1x 2‑cell LiPo battery (7.4 V nominal) with switch and suitable connector
• Assorted wiring: dupont jumpers, screw terminals
• Small hardware: standoffs, mounting tape, heatshrink
• A chassis supporting two wheels in a segway‑like configuration
• Optional:
• 5.5×2.1 mm barrel jack or XT30/XT60 pigtail
• Inline fuse (5–10 A) in series with battery
• Bench power supply for initial motor tests

---

Setup/Connection

Use a single power domain for logic (3.3 V from Nano) and a separate motor power rail (VM from LiPo). The TB6612FNG supports logic VCC = 2.7–5.5 V (set to 3.3 V for Nano BLE compatibility) and motor VM = up to ~13.5 V. Always share grounds.

• Power domains and grounds:
• Nano 33 BLE Sense 3.3 V pin → TB6612FNG VCC
• Nano 33 BLE Sense GND → TB6612FNG GND and AS5600 GND
• LiPo positive → TB6612FNG VM

• LiPo negative → TB6612FNG GND (this ties to logic ground)

• TB6612FNG motor wiring:

• Motor A → A01/A02; Motor B → B01/B02

• STBY pulled high via a GPIO (we’ll control it from firmware)

• AS5600 I2C encoder:

• AS5600 VCC → 3.3 V
• AS5600 GND → GND
• AS5600 SDA → Nano SDA (labeled “SDA” on the header)
• AS5600 SCL → Nano SCL (labeled “SCL” on the header)

• Place the diametric magnet centered on the wheel/shaft so the AS5600 is coaxial and close enough for strong magnetic field reading.

• Orientation:

• Mount the Nano 33 BLE Sense so its IMU axes are consistent with your pitch axis. In this guide, we compute pitch mostly around the “board Y” gyro axis, combining with the X/Z accelerometer vectors. If your board orientation differs, you’ll adjust sign axes during validation.

Pin mapping summary

The table below provides a concrete, tested mapping for this project. All logic is 3.3 V.

Tip: If your motor polarity is reversed, swap AIN1/AIN2 or BIN1/BIN2 (or swap motor leads), keeping the code consistent.

---

Full Code

Below is a complete PlatformIO project definition (platformio.ini) and the main firmware (src/main.cpp). The code implements:

• IMU initialization and gyro bias calibration
• Complementary filter for pitch angle estimation (accelerometer + gyroscope)
• AS5600 I2C angle read, unwrap, and speed estimate
• Cascaded control: speed loop → target pitch, and angle PID → motor command
• Safety interlocks: motor disable on large tilt or invalid sensors
• Runtime telemetry over serial at 115200 baud

platformio.ini

; PlatformIO configuration for Arduino Nano 33 BLE Sense
; Tested with PlatformIO Core 6.1.11 and platform nordicnrf52 10.4.0
; Save this file at: <project-root>/platformio.ini

[env:nano33ble]
platform = nordicnrf52@10.4.0
board = nano33ble
framework = arduino
monitor_speed = 115200
lib_deps =
  arduino-libraries/Arduino_LSM9DS1@^1.1.0

; Optional: speed up Wire I2C, and expose some build-time config flags
build_flags =
  -D PITCH_SAFETY_DEG=45.0
  -D MOTOR_MAX_PWM=255
  -D SERIAL_BAUD=115200

src/main.cpp

// Save as: src/main.cpp
#include <Arduino.h>
#include <Wire.h>
#include <Arduino_LSM9DS1.h>  // IMU (LSM9DS1) library provided by arduino-libraries

// ----------------- Pin map (see table in guide) -----------------
static const int PIN_PWMA = D3;   // Left motor PWM
static const int PIN_AIN1 = D4;
static const int PIN_AIN2 = D5;

static const int PIN_PWMB = D10;  // Right motor PWM
static const int PIN_BIN1 = D8;
static const int PIN_BIN2 = D9;

static const int PIN_STBY = D6;   // TB6612FNG standby

// ----------------- AS5600 (I2C) -----------------
static const uint8_t AS5600_ADDR    = 0x36;
static const uint8_t AS5600_ANGLE_H = 0x0E;
static const uint8_t AS5600_ANGLE_L = 0x0F;
static const float   TWO_PI_F       = 6.28318530718f;
static const float   AS5600_LSB_TO_RAD = TWO_PI_F / 4096.0f;

// ----------------- Control parameters -----------------
// Angle loop (inner) — set conservative defaults; tune later
volatile float Kp_angle = 18.0f;
volatile float Ki_angle = 0.0f;
volatile float Kd_angle = 0.5f;

// Speed loop (outer) — target wheel speed -> target pitch
volatile float Kp_speed = 0.6f;
volatile float Ki_speed = 0.2f;
volatile float Kd_speed = 0.0f;

volatile float speed_target_rads = 0.0f;  // desired wheel speed (rad/s), set via serial or 0 for in-place balance

// Complementary filter blending (gyro integration vs accelerometer)
volatile float complementary_alpha = 0.98f;

// Safety and scaling
#ifndef PITCH_SAFETY_DEG
#define PITCH_SAFETY_DEG 45.0
#endif
#ifndef MOTOR_MAX_PWM
#define MOTOR_MAX_PWM 255
#endif
#ifndef SERIAL_BAUD
#define SERIAL_BAUD 115200
#endif

// ----------------- State variables -----------------
static float gyroBiasY = 0.0f;
static bool  motorsEnabled = false;

static float pitch_deg = 0.0f;           // filtered pitch
static float lastPitchDeg = 0.0f;
static float integral_angle = 0.0f;
static float lastAngleErr = 0.0f;

static float integral_speed = 0.0f;
static float lastSpeedErr = 0.0f;

static uint16_t as5600_last_raw = 0;
static float as5600_angle_rad = 0.0f;    // unwrapped angle
static float as5600_last_angle_rad = 0.0f;
static float as5600_speed_rads = 0.0f;

// Loop timing
static const float LOOP_HZ = 400.0f;      // 400 Hz control loop
static const uint32_t LOOP_US = (uint32_t)(1000000.0f / LOOP_HZ);
static uint32_t nextLoop = 0;

// Utility: saturate
template<typename T>
static inline T clamp(T v, T lo, T hi) { return (v < lo) ? lo : (v > hi) ? hi : v; }

// ----------------- TB6612FNG motor control -----------------
void driverStandby(bool enable) {
  digitalWrite(PIN_STBY, enable ? HIGH : LOW);
}

void setMotorRaw(int pwm, int in1, int in2, int speed) {
  // speed: [-255..255]
  int s = clamp(speed, -MOTOR_MAX_PWM, MOTOR_MAX_PWM);
  if (s >= 0) {
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    analogWrite(pwm, s);
  } else {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    analogWrite(pwm, -s);
  }
}

void setMotors(int left, int right) {
  setMotorRaw(PIN_PWMA, PIN_AIN1, PIN_AIN2, left);
  setMotorRaw(PIN_PWMB, PIN_BIN1, PIN_BIN2, right);
}

void stopMotors() {
  analogWrite(PIN_PWMA, 0);
  analogWrite(PIN_PWMB, 0);
  // Brake by shorting both direction pins the same, optional:
  digitalWrite(PIN_AIN1, LOW); digitalWrite(PIN_AIN2, LOW);
  digitalWrite(PIN_BIN1, LOW); digitalWrite(PIN_BIN2, LOW);
}

// ----------------- AS5600 read helpers -----------------
uint16_t as5600ReadRaw12() {
  Wire.beginTransmission(AS5600_ADDR);
  Wire.write(AS5600_ANGLE_H);
  if (Wire.endTransmission(false) != 0) return as5600_last_raw; // NACK -> return last
  Wire.requestFrom((int)AS5600_ADDR, 2);
  if (Wire.available() < 2) return as5600_last_raw;
  uint8_t hi = Wire.read();
  uint8_t lo = Wire.read();
  uint16_t raw = ((uint16_t)hi << 8) | lo;
  raw &= 0x0FFF; // 12-bit
  as5600_last_raw = raw;
  return raw;
}

void as5600Update(float dt) {
  uint16_t raw = as5600ReadRaw12();
  float angle_now = (float)raw * AS5600_LSB_TO_RAD;

  // unwrap across 0..2pi
  float delta = angle_now - fmodf(as5600_last_angle_rad, TWO_PI_F);
  if (delta > M_PI)  delta -= TWO_PI_F;
  if (delta < -M_PI) delta += TWO_PI_F;

  as5600_angle_rad += delta;      // unwrapped absolute angle
  as5600_speed_rads = delta / dt; // instantaneous speed
  as5600_last_angle_rad = as5600_angle_rad;
}

// ----------------- IMU initialization and filters -----------------
bool imuInit() {
  if (!IMU.begin()) return false;

  // Gyro bias calibration: keep still
  const uint32_t calibMs = 3000;
  const uint32_t start = millis();
  float sumGy = 0.0f;
  int samples = 0;

  while (millis() - start < calibMs) {
    float gx, gy, gz;
    if (IMU.gyroscopeAvailable() && IMU.readGyroscope(gx, gy, gz)) {
      sumGy += gy;
      samples++;
    }
    digitalWrite(LED_BUILTIN, (millis() / 200) % 2); // blink LED during calibration
  }
  digitalWrite(LED_BUILTIN, LOW);

  if (samples > 20) {
    gyroBiasY = sumGy / samples;
  } else {
    gyroBiasY = 0.0f;
  }
  return true;
}

bool imuUpdate(float dt) {
  float ax, ay, az, gx, gy, gz;
  bool gotAcc = IMU.accelerationAvailable();
  bool gotGyr = IMU.gyroscopeAvailable();
  if (gotAcc) gotAcc = IMU.readAcceleration(ax, ay, az);
  if (gotGyr) gotGyr = IMU.readGyroscope(gx, gy, gz);
  if (!gotAcc || !gotGyr) return false;

  // Units: acceleration in g (per Arduino_LSM9DS1), gyro in dps
  // Pitch from accelerometer: project X vs Z (board orientation dependent)
  float pitch_acc_deg = atan2f(ax, az) * 180.0f / PI;

  // Gyro Y (board axis) as pitch rate — adjust sign if needed
  float gyroY_dps = gy - gyroBiasY;
  float pitch_pred = pitch_deg + gyroY_dps * dt; // integrate gyro

  // Complementary filter
  pitch_deg = complementary_alpha * pitch_pred + (1.0f - complementary_alpha) * pitch_acc_deg;
  return true;
}

// ----------------- Control -----------------
void controlUpdate(float dt) {
  // Safety: disable motors if fallen
  if (fabsf(pitch_deg) > PITCH_SAFETY_DEG) {
    motorsEnabled = false;
    stopMotors();
    return;
  }

  // Outer loop: wheel speed -> target pitch (deg) to keep position/speed
  float speed_err = speed_target_rads - as5600_speed_rads;
  integral_speed += speed_err * dt;
  integral_speed = clamp(integral_speed, -10.0f, 10.0f); // prevent windup
  float deriv_speed = (speed_err - lastSpeedErr) / dt;
  lastSpeedErr = speed_err;

  float pitch_target_deg = (Kp_speed * speed_err) + (Ki_speed * integral_speed) + (Kd_speed * deriv_speed);
  pitch_target_deg = clamp(pitch_target_deg, -10.0f, 10.0f); // keep within reasonable tilt

  // Inner loop: angle PID -> motor command
  float angle_err = pitch_target_deg - pitch_deg;
  integral_angle += angle_err * dt;
  integral_angle = clamp(integral_angle, -20.0f, 20.0f); // anti-windup
  float deriv_angle = (angle_err - lastAngleErr) / dt;
  lastAngleErr = angle_err;

  float u = (Kp_angle * angle_err) + (Ki_angle * integral_angle) + (Kd_angle * deriv_angle);

  // Map control effort to motor PWM
  int pwm = (int)clamp(u, (float)-MOTOR_MAX_PWM, (float)MOTOR_MAX_PWM);

  if (motorsEnabled) {
    // For straight balance, same command for both sides
    setMotors(pwm, pwm);
  } else {
    stopMotors();
  }
}

// ----------------- Serial command parser -----------------
void printHelp() {
  Serial.println(F("Commands:"));
  Serial.println(F("  start               -> enable motors"));
  Serial.println(F("  stop                -> disable motors"));
  Serial.println(F("  kp <val>            -> set Kp_angle"));
  Serial.println(F("  ki <val>            -> set Ki_angle"));
  Serial.println(F("  kd <val>            -> set Kd_angle"));
  Serial.println(F("  spkp <val>          -> set Kp_speed"));
  Serial.println(F("  spki <val>          -> set Ki_speed"));
  Serial.println(F("  spkd <val>          -> set Kd_speed"));
  Serial.println(F("  vel <rad/s>         -> set speed_target_rads"));
  Serial.println(F("  alpha <0..1>        -> set complementary alpha"));
  Serial.println(F("  status              -> print runtime status"));
  Serial.println(F("  help                -> show this help"));
}

void printStatus() {
  Serial.print(F("# "));
  Serial.print(F("pitch=")); Serial.print(pitch_deg, 3);
  Serial.print(F(" deg, speed=")); Serial.print(as5600_speed_rads, 3);
  Serial.print(F(" rad/s, target_vel=")); Serial.print(speed_target_rads, 3);
  Serial.print(F(", K=[ ")); Serial.print(Kp_angle, 3); Serial.print(' ');
  Serial.print(Ki_angle, 3); Serial.print(' '); Serial.print(Kd_angle, 3);
  Serial.print(F(" ], SPK=[ ")); Serial.print(Kp_speed, 3); Serial.print(' ');
  Serial.print(Ki_speed, 3); Serial.print(' '); Serial.print(Kd_speed, 3);
  Serial.print(F(" ], alpha=")); Serial.print(complementary_alpha, 3);
  Serial.print(F(", motors=")); Serial.println(motorsEnabled ? F("ON") : F("OFF"));
}

void handleSerial() {
  static String line;
  while (Serial.available()) {
    char c = (char)Serial.read();
    if (c == '\r') continue;
    if (c == '\n') {
      line.trim();
      if (line.length() == 0) { line = ""; continue; }

      if (line.equalsIgnoreCase("help")) {
        printHelp();
      } else if (line.equalsIgnoreCase("start")) {
        motorsEnabled = true;
        driverStandby(true);
        Serial.println(F("Motors enabled."));
      } else if (line.equalsIgnoreCase("stop")) {
        motorsEnabled = false;
        stopMotors();
        Serial.println(F("Motors disabled."));
      } else if (line.startsWith("kp ")) {
        Kp_angle = line.substring(3).toFloat();
        Serial.println(F("OK"));
      } else if (line.startsWith("ki ")) {
        Ki_angle = line.substring(3).toFloat();
        Serial.println(F("OK"));
      } else if (line.startsWith("kd ")) {
        Kd_angle = line.substring(3).toFloat();
        Serial.println(F("OK"));
      } else if (line.startsWith("spkp ")) {
        Kp_speed = line.substring(5).toFloat();
        Serial.println(F("OK"));
      } else if (line.startsWith("spki ")) {
        Ki_speed = line.substring(5).toFloat();
        Serial.println(F("OK"));
      } else if (line.startsWith("spkd ")) {
        Kd_speed = line.substring(5).toFloat();
        Serial.println(F("OK"));
      } else if (line.startsWith("vel ")) {
        speed_target_rads = line.substring(4).toFloat();
        Serial.println(F("OK"));
      } else if (line.startsWith("alpha ")) {
        complementary_alpha = line.substring(6).toFloat();
        complementary_alpha = clamp(complementary_alpha, 0.0f, 1.0f);
        Serial.println(F("OK"));
      } else if (line.equalsIgnoreCase("status")) {
        printStatus();
      } else {
        Serial.println(F("ERR: unknown command (type 'help')"));
      }
      line = "";
    } else {
      line += c;
      if (line.length() > 120) line = ""; // guard
    }
  }
}

// ----------------- Setup/loop -----------------
void setup() {
  pinMode(LED_BUILTIN, OUTPUT);

  pinMode(PIN_STBY, OUTPUT);
  pinMode(PIN_AIN1, OUTPUT);
  pinMode(PIN_AIN2, OUTPUT);
  pinMode(PIN_BIN1, OUTPUT);
  pinMode(PIN_BIN2, OUTPUT);

  // Pre-brake and standby low
  stopMotors();
  driverStandby(false);

  Serial.begin(SERIAL_BAUD);
  while (!Serial && millis() < 2000) { /* wait for CDC */ }

  Wire.begin();
  Wire.setClock(400000); // 400 kHz I2C for AS5600

  if (!imuInit()) {
    Serial.println(F("IMU init failed! Check board and library."));
  } else {
    Serial.println(F("IMU ready."));
  }

  // Initialize AS5600
  as5600_last_raw = as5600ReadRaw12();
  as5600_last_angle_rad = (float)as5600_last_raw * AS5600_LSB_TO_RAD;
  as5600_angle_rad = as5600_last_angle_rad;

  printHelp();
  printStatus();

  nextLoop = micros() + LOOP_US;
}

void loop() {
  // Maintain fixed loop rate
  const uint32_t now = micros();
  if ((int32_t)(now - nextLoop) < 0) {
    handleSerial();
    return;
  }
  const uint32_t lastLoop = nextLoop - LOOP_US;
  nextLoop += LOOP_US;

  float dt = (now - lastLoop) / 1000000.0f;
  dt = clamp(dt, 0.0005f, 0.01f); // 0.5..10 ms guard

  bool imu_ok = imuUpdate(dt);
  as5600Update(dt);

  if (!imu_ok) {
    // If IMU sampling fell behind, temporarily stop motors
    stopMotors();
  } else {
    controlUpdate(dt);
  }

  // Telemetry at ~50 Hz
  static uint32_t lastTelem = 0;
  if (millis() - lastTelem >= 20) {
    lastTelem = millis();
    Serial.print(F("T,"));
    Serial.print(pitch_deg, 3); Serial.print(',');
    Serial.print(as5600_speed_rads, 3); Serial.print(',');
    Serial.print(speed_target_rads, 3); Serial.print(',');
    Serial.print(Kp_angle, 3); Serial.print(',');
    Serial.print(Ki_angle, 3); Serial.print(',');
    Serial.print(Kd_angle, 3); Serial.print(',');
    Serial.println(motorsEnabled ? 1 : 0);
  }
}

Notes:
– If your pitch direction is inverted, change either atan2f(ax, az) to -atan2f(ax, az) or invert gyroY_dps sign consistently.
– The complementary filter alpha, angle PID, and speed gains require tuning; start conservative.

---

Build/Flash/Run commands

Use PlatformIO CLI. The steps below are exact and reproducible.

python3 -m pip install --upgrade pip
python3 -m pip install --user platformio

# Verify version (expect 6.1.11 or newer)
pio --version

# 2) Create a project directory and place platformio.ini and src/main.cpp
mkdir -p ~/work/imu-self-balancer
cd ~/work/imu-self-balancer
# (copy the provided platformio.ini and src/main.cpp into this folder)

# 3) Initialize (optional; platformio.ini already defines env)
pio project init --board nano33ble

# 4) Fetch/update libs and platforms
pio pkg update

# 5) Build
pio run -e nano33ble

# 6) Find your serial/USB port
pio device list
# Example output: /dev/ttyACM0 (Linux/macOS) or COM5 (Windows)

# 7) Upload (auto-detects port)
pio run -e nano33ble -t upload

# Alternatively, upload specifying the port explicitly:
# Linux/macOS:
pio run -e nano33ble -t upload --upload-port /dev/ttyACM0
# Windows:
pio run -e nano33ble -t upload --upload-port COM5

# 8) Open serial monitor at 115200 baud
pio device monitor -b 115200

# Optional: log telemetry for analysis (Linux/macOS)
pio device monitor -b 115200 | tee session.log
# Optional: PowerShell
# pio device monitor -b 115200 | Tee-Object -FilePath session.log

Driver notes:
– The Nano 33 BLE Sense uses native USB CDC; on Windows 10+ it appears as a COM device without extra drivers. If upload fails, double‑tap the board’s reset button to re‑enter the bootloader, then retry upload.

---

Step‑by‑step Validation

Follow these steps in order, validating each subsystem before attempting full balancing.

1) Power and wiring sanity check
– Disconnect the LiPo. Power only the Nano over USB.
– With a multimeter:
– TB6612FNG VCC = 3.3 V (from Nano), GND shared.
– AS5600 VCC = 3.3 V, GND shared, SDA/SCL to the correct header pins.
– Ensure motor VM remains unpowered for now.

2) IMU bring‑up
– Open the monitor: pio device monitor -b 115200.
– Reset the board. You should see “IMU ready.” and the command help.
– Keep the board still for ~3 seconds after reset to allow gyro bias calibration (LED blinks).
– Observe telemetry lines starting with T,. If pitch is static near 0 when upright, good.
– Gently tilt the board forward/backward; pitch should change smoothly, with gyro dynamics evident.
– If pitch increases in the wrong direction, invert pitch sign in code as noted.

3) AS5600 encoder validation
– Slowly rotate the wheel/shaft with the magnet and AS5600 aligned.
– Watch telemetry: the second value (speed rad/s) should go positive for one direction, negative for the other.
– If speed seems noisy, try slowing I2C to 100 kHz (Wire.setClock(100000)) as a test, or increase the loop time.
– If zeros only, check:
– I2C address (0x36) correct
– Ground/SDA/SCL continuity
– Magnet spacing and centering (the AS5600 needs a strong, centered field)

4) TB6612FNG static test
– With USB only, send “start” then quickly “stop”. Motors won’t turn (no VM), but the driver should exit standby (voltage present on STBY).
– Use a multimeter or logic probe: verify AIN1/AIN2/BIN1/BIN2 toggle when PWM is nonzero.
– Send “vel 0.0” to ensure speed target is 0 during initial tests.

5) Motor bring‑up with VM
– Connect LiPo to TB6612FNG VM and GND. Ensure correct polarity and an inline fuse.
– Lift wheels off the ground. Send “start” and gently set a small “vel 0.5”.
– Both wheels should rotate in the same direction. If not:
– Reverse either AIN1/AIN2 or motor leads for the left side.
– Repeat for right side with BIN1/BIN2 or motor leads.
– The goal is that positive command drives the rover forward.

6) First balancing attempt (assisted)
– Place the rover between your hands, wheels on a flat surface.
– Set “vel 0.0”; send “start”.
– With default gains, it should attempt to correct pitch. If it oscillates rapidly or runs away, hit “stop” and proceed to tuning.
– Aim for quick damping: a few small overshoots but converging toward upright.

7) Tuning methodology
– Angle loop first (speed loop disabled): set Kp_speed = 0, Ki_speed = 0, Kd_speed = 0.
– Increase Kp_angle until the rover barely oscillates when displaced (critical-ish).
– Add Kd_angle to reduce overshoot and sharpen response; start with 0.5 and increase in 0.1 steps.
– Add a small Ki_angle (e.g., 0.05–0.2) only if there’s steady bias (it will fight gravity offset).
– Speed loop next:
– Set a small Kp_speed (0.3–0.8). It biases the target pitch to counter velocity drift.
– Add Ki_speed (0.05–0.3) to remove residual drift over time.
– Usually Kd_speed can remain 0 for a simple platform.
– Complementary filter alpha:
– Increase alpha (closer to 1) if the platform reacts too nervously to accel noise.
– Decrease alpha if you see slow drift due to gyro bias (but do not go too low; 0.95–0.99 is typical).

8) Closed‑loop validation
– With the tuned gains, the rover should stand with minimal hand assistance.
– Nudge it forward/backward: it should briefly tilt to accelerate and then recover to upright.
– Try small “vel 0.3” forward/back; the rover should roll slowly while staying balanced.
– Confirm safety: forcibly tip beyond 45°; motors should shut off.

9) Data logging and analysis
– Log: pio device monitor -b 115200 | tee log.csv
– Columns in telemetry (T,…) are: pitch_deg, wheel_speed_rads, speed_target_rads, Kp, Ki, Kd, motors_on
– Plot pitch vs time and correlate with motor commands (not printed but visible via behavior).
– Refine Kd_angle and alpha to balance noise rejection vs responsiveness.

---

Troubleshooting

• Upload fails or port missing:
• Double‑tap reset on the Nano 33 BLE Sense to enter the bootloader; retry upload.
• Use pio device list to confirm the port; pass –upload-port explicitly.

• Check cable (some charge‑only cables lack data lines).

• IMU reads are stale or freezing:

• Ensure Serial prints are not saturating USB; telemetry is already throttled to ~50 Hz.
• Avoid delay() in loop; this code uses a timed loop with micros().

• Power noise from motors can brown out the board; add a large low‑ESR capacitor (e.g., 470 µF–1000 µF) across VM/GND and keep grounds star‑connected at the driver.

• AS5600 gives 0 or random values:

• Verify 3.3 V supply (do not power at 5 V).
• Confirm magnet alignment and spacing; the sensor must see a strong, centered field.
• Reduce I2C speed (Wire.setClock(100000)) for long wires.

• If you have two encoders, remember the default I2C address is the same (0x36); many breakouts don’t allow address change. Use separate buses or mux if adding more.

• Motors don’t spin or spin only one direction:

• Check STBY pin is driven high (driverStandby(true) after “start”).
• Verify PWM pins are indeed PWM capable (D3, D10 are).

• Swap AIN1/AIN2 or BIN1/BIN2 if direction is inverted.

• Violent oscillations:

• Kp_angle too high or Kd_angle too low. Reduce Kp_angle by 20%; increase Kd_angle by 0.1–0.2.
• Increase complementary_alpha slightly (e.g., from 0.98 to 0.985).

• Make sure chassis has a low center of gravity directly above the axle line.

• Slow drift or won’t stand upright for long:

• Add a small Ki_angle.
• Use speed loop (Kp_speed, Ki_speed) to trim off drift due to wheel friction or floor slope.

• Re‑run with the board absolutely still at power‑up to improve gyro bias estimation.

• Brownouts / USB disconnects when motors start:

• VM must not be drawn from the Nano 3.3 V. Separate motor supply (LiPo) with common ground only.
• Add supply decouplers near TB6612FNG VM and a Schottky clamp if needed.
• Use twisted pairs for motor leads and keep high current away from signal lines.

---

Improvements

• Dual encoders: Add a second AS5600 for the other wheel. Use an I2C multiplexer (TCA9548A) or a second I2C bus to avoid address conflict. This enables speed/yaw control and better straight‑line performance.
• Yaw stabilization: Add a simple yaw PI loop using gyro Z to reduce heading drift at zero speed.
• Better state estimation: Replace complementary filter with a Mahony/Madgwick or an EKF incorporating wheel odometry to estimate pitch bias and drift more robustly.
• Adaptive gains: Reduce Kp_angle as speed increases to avoid over‑aggressive corrections at higher velocities.
• Current/voltage sensing: Add INA219/INA260 to monitor VM and current draw; use it to implement brownout‑aware derating.
• Motion profiles: Implement trapezoidal or S‑curves for speed targets (vel command) to reduce jerk during transitions.
• Safety layers: Add a hardware interlock using a latching relay or MOSFET cutoff triggered by a dedicated “kill” input.

---

Final Checklist

• Materials
• Arduino Nano 33 BLE Sense, TB6612FNG, AS5600 with magnet, LiPo, two DC gearmotors, wiring
• Wiring
• TB6612FNG VCC=3.3 V, VM=LiPo+, GND common; STBY to D6; PWM: D3 (left), D10 (right); DIR: D4/D5 left, D8/D9 right
• AS5600 on I2C SDA/SCL at 3.3 V
• Motors correctly wired to A01/A02 and B01/B02; directions verified
• Software
• PlatformIO Core installed (pio –version shows 6.1.11+)
• platformio.ini and src/main.cpp exactly as provided
• Libraries resolved (Arduino_LSM9DS1)
• Build/Upload
• pio run -e nano33ble success
• pio run -e nano33ble -t upload success; serial monitor at 115200 works
• Validation
• IMU pitch changes logically with tilt; complementary filter stable
• AS5600 speed rad/s reflects wheel motion both directions
• TB6612FNG responds to start/stop commands; wheels turn with VM connected
• Control
• Safety threshold enforced (±45°)
• Angle loop tuned to near‑critical damping
• Speed loop trims drift; vel commands produce controlled motion
• Documentation/Logs
• Telemetry saved for tuning; final gains recorded
• Known orientation/sign conventions documented in code

You now have a repeatable, CLI‑driven build for an advanced IMU self‑balancing rover centered on the exact device model: Arduino Nano 33 BLE Sense + TB6612FNG + AS5600.