Self-Balancing Triangle

Firmware for a self-balancing reaction wheel robot, powered by the Raspberry Pi Pico W (RP2040).

This project uses Field Oriented Control (FOC) to drive a brushless DC motor, maintaining the triangle's balance on a single point. It features Bluetooth Low Energy (BLE) connectivity for real-time parameter tuning and telemetry.

Features

• Field Oriented Control (FOC): Implements commutation for the BLDC motor using Space Vector Pulse Width Modulation (SVPWM) (via Inverse Park/Clarke transforms).
• Dual-Core Architecture:
  • Core 0: Handles the control loop, and BLE communication.
  • Core 1: Dedicated to the high-frequency motor control loop (FOC/Commutation) to ensure smooth motor operation.
• Auto-Calibration: Automatically calibrates the gyroscope on startup and supports motor encoder alignment.
• Bluetooth LE Tuning: Modify gains (k1, k2, k3) and target angle wirelessly using a BLE-enabled device.
• Low Battery Protection: Measures voltage with ADC using voltage divider.

Hardware Requirements

Microcontroller

• Raspberry Pi Pico W

Sensors

• IMU: MPU6050 (Accelerometer + Gyroscope)
  • Communication: I2C
  • SDA: GPIO 26
  • SCL: GPIO 27
• Motor Encoder: AS5047 / AS5147 (Magnetic Rotary Position Sensor)
  • Communication: SPI
  • SCK: GPIO 2
  • MOSI: GPIO 3
  • MISO: GPIO 4
  • CS: GPIO 5

Actuator

• BLDC Motor: iPower GM3506 (11 pole pairs).
• Driver: 3-Phase BLDC Motor Driver
  • PWM Phase A: GPIO 10
  • PWM Phase B: GPIO 11
  • PWM Phase C: GPIO 12
  • Enable Pin: GPIO 13

Power

• Battery: 3S LiPo (approx. 11.1V - 12.6V)
• Voltage Sense: Voltage divider connected to GPIO 28 (ADC2). The code assumes a divider ratio of 5.

Software Architecture

The software is structured into modular components:

• src/main.cpp: The entry point. It initializes hardware, launches the motor loop on Core 1, and runs the main control loop on Core 0.
• src/hardware/:
  • AngleSensor: Handles MPU6050 communication, data fusion (Complementary filter), and calibration.
  • Motor: Implements the FOC algorithm, controlling the 3-phase voltage to generate torque.
  • MotorSensor: Reads absolute angle from the SPI magnetic encoder.
  • Driver: Generates 3-phase PWM signals.
  • Battery: Monitors battery voltage.
• src/controller/:
  • Controller: Implements the state-feedback controller u = k1*angle + k2*gyro + k3*motor_speed. It also handles logic to adjust targetAngle.
  • Storage: Saves/Restores calibration data and tuning parameters to flash memory.
• src/bluetooth/:
  • Communication: Manages the BLE stack, GATT services, and handles incoming parameter updates.

Building the Project

This project uses the Raspberry Pi Pico SDK and CMake.

1. Install Prerequisites:

   • CMake
   • Arm Toolchain (arm-none-eabi-gcc)
   • Raspberry Pi Pico SDK (Set PICO_SDK_PATH environment variable)

2. Build:

   mkdir build
   cd build
   cmake ..
   make

3. Flash:

   • Hold the BOOTSEL button on the Pico W and plug it in via USB.
   • Copy the generated triangle.uf2 file to the mounted RPI-RP2 drive.

Debugging

The project includes an openocd_pico.cfg configuration file for debugging with OpenOCD and GDB via the SWD interface. It is configured for CMSIS-DAP adapters (e.g., Raspberry Pi Debug Probe or another Pico running picoprobe).

To start an OpenOCD session:

openocd -f openocd_pico.cfg

You can then connect with GDB:

arm-none-eabi-gdb build/triangle.elf
(gdb) target remote localhost:3333
(gdb) load
(gdb) monitor reset init
(gdb) continue

Bluetooth Interface

The robot exposes a BLE service for real-time tuning. You can use a generic BLE Scanner app or a custom dashboard to connect.

Telemetry

• Status String: Contains Target Angle, Current Angle, and Battery Voltage.

Writable Parameters (Float)

• Index 1: k1 (Angle Gain)
• Index 2: k2 (Angular Velocity Gain)
• Index 3: k3 (Motor Speed Gain)
• Index 4: targetAngle (Manual offset for balance point)

License

See LICENSE file.