Project files
20 KB
Machine-translated by Robotheca from the original in Chinese (opens in a new tab). License: MIT (opens in a new tab).
Preface
I graduated in Automation, during which I studied various principles of motor motion control and automatic control, but I only knew how to pass exams without truly understanding the underlying principles. Recently, I came across SimpleFOC, an open-source brushless motor driver library based on Arduino. I wanted to take this opportunity to apply what I learned in college to a practical project, which led to this small project: a balancing robot based on FOC-controlled brushless motors.
Compared to brushed motors commonly used in small cars, brushless motors offer greater torque at the same size. When driving a car, brushless motors can be directly coupled without the need for a gearbox to meet torque requirements, and they also respond faster. Using brushless motors allows the car to be made more compact and aesthetically pleasing.
The driving circuit and algorithm for brushless motors are more complex. When operating at low speeds, the Field-Oriented Control (FOC) algorithm is used at the algorithmic level to precisely control the motor's position, speed, and even current (i.e., torque), enabling the creation of many fun applications. FOC-based brushless motors are widely used in areas such as robotic joints and camera gimbal motor control.
Main Features
- Onboard dual-channel brushless motor driver and current-loop chip, with a maximum current of 2.5A per channel. It can drive two gimbal brushless motors and control them down to the current level.
- The car can self-balance and can be controlled to move forward, backward, and steer via a mobile app over Bluetooth.
- Equipped with dual-row RGB LEDs, with 24 LEDs per row (expandable to more), enabling various lighting effects without affecting vehicle control.
- Video transmission interface, using an external ESP-CAM module for Wi-Fi video transmission, viewable on a mobile phone (as of 20220421, the port is reversed and not usable).
Open-source Resources Links
Blog documentation link https://blog.csdn.net/weixin_42487906/article/details/124898392 (opens in a new tab)
Video link https://www.bilibili.com/video/BV1Sv4y137FG/ (opens in a new tab)
GitHub repository, containing code, 3D printing files, and PCB design files: https://github.com/FranHawk/simplefoc_balance_car (opens in a new tab)
LCEDA project (can view schematics and PCBs online): There are three boards in total Main board (includes motor control and driver section): https://oshwhub.com/FranHawk/balance_car_esp32_simplefoc (opens in a new tab) Encoder board: https://oshwhub.com/FranHawk/as5600-16mm (opens in a new tab) RGB LED strip board: https://oshwhub.com/FranHawk/rgb_strip (opens in a new tab)
Purchase Links for Materials
All electronic components can be purchased by referring to the BOM table or the schematics in the project. You can buy them on Taobao or LCSC. Below are purchase links for some key materials.
AS5600 L6234PD INA240A1: Taobao (opens in a new tab) "L6234PD L6234D L6234 HSOP Original Automotive IC Imported Chip Hot Sale High Quality" Click the link to open directly.
This seller offers relatively affordable motor driver chips.
Other electronic components can be found on stores like Youxin Electronics. If your budget allows, you can directly purchase from LCSC for convenience.
2210 Gimbal Motor (at least two): Taobao (opens in a new tab) "Clearance Sale HJ2210-75T Brushless Gimbal Motor Dog 3 Gimbal Special Aerial Photography Motor Gimbal Hollow Shaft" Click the link to open directly.
Tire Treads (at least two): Taobao (opens in a new tab) "AXIAL 1:24 Car SCX24 90081 Xiaomi Jimny Universal Rubber Climbing Tire Tread Diameter 52mm" Click the link to open directly.
Motor Mounts (at least two): Taobao (opens in a new tab) "390 Motor Mount 385 Mounting Base 365 Mounting Base 380 Motor Mount Includes Screws and Nuts" Click the link to open directly.
Black Acrylic Board (at least two): Taobao (opens in a new tab) "Balance Car DIY Special Acrylic Support Plate Expansion Board" Click the link to open directly.
Battery 900mAh: Taobao (opens in a new tab) "RC Model Battery 5200mAh 11.1V High Capacity High C Rating 7.4V Car Model Ship Model RC Climbing Car Quality Guaranteed" Click the link to open directly.
Other Small Mechanical Parts
- 1.0 mm wires, 3P and 4P. Note: Before wiring, make sure the wiring sequence of the encoder board and the main board matches. The wires you buy directly cannot be used and need to be soldered and rearranged.
- M2.5 screws, at least 8 mm long, and several M2.5 washers, used to secure the encoder, motor, and motor mount.
- M3 6 mm screws, M35 6 mm copper pillars, M320 6 mm copper pillars, and several M3 nuts.
- AB glue or glue of equivalent strength, used to secure the radial magnet on the motor shaft.
- Cable ties and Velcro, used to secure the main board and battery.
- 3.81 mm connectors, used to connect the motor wires to the main board. XT30 male connector, used to connect the main board to the battery. Red and black power wires.
Manufacturing Steps
1. Circuit Section
Open the three PCB projects linked in the open-source materials, have them produced at JLCPCB, purchase and solder all the components.
Main Board
Image: Main board top view (opens in a new tab) Image: Main board bottom view (opens in a new tab) Solder the main board to the XT30 male connector using red and black power wires as shown in the images above.
Encoder Board
Image: Encoder board (opens in a new tab) The encoder board has few components. Remember to solder the point shown in the image. There is an AS5600 chip on the back; two boards need to be made in total.
RGB Light Board
Image: RGB light board (opens in a new tab) Solder the LEDs onto the board; nothing else to note. Two boards need to be made in total.
SH1.0 3P and 4P Wires
Make two of each to connect the RGB and encoder boards to the main board. Before wiring, be sure to check that the pin order on the encoder board matches the main board. I received boards with reversed order, so the wire sequence had to be adjusted accordingly.
Mechanical Parts
Make two motor assemblies
Image: Insert image description here (opens in a new tab)
First, glue the radial magnets onto the motor shaft center
Image: Insert image description here (opens in a new tab)
Assemble the encoder board, motor bracket, and motor using M2.5 screws as shown in the diagram below. Add some washers in between to prevent friction between parts, as shown below
Image: Insert image description here (opens in a new tab)
3D print the wheel hub, fit it onto the motor, then add the tire skin. The 3D printing files are available on GitHub
Assembly
3D print the mounting plate, secure the battery with Velcro and zip ties, and assemble the 3D-printed mounting plate with the mainboard using M3×5 brass standoffs and screws.
Image: Assembly step 1 (opens in a new tab) Image: Assembly step 2 (opens in a new tab)
Assemble the two motor sections with the mainboard using M3 screws. Connect the motor wires to the mainboard via 3.8 mm connectors in any order. Connect the encoder board using a 1.0 mm SH 4P cable. Mount four M3×20 mm brass standoffs on the first acrylic layer as shown below.
Image: Assembly step 3 (opens in a new tab)
Glue the RGB light board to the second acrylic layer.
Image: Assembly step 4 (opens in a new tab)
Connect the RGB wiring to the mainboard, secure the first and second acrylic layers, and complete the assembly.
Flashing the Program
Use VSCode + PlatformIO to flash the program.
Before flashing for the first time, locate the hw_bldc_init() function in hw_bldc.cpp. You will see the following lines:
motor_0.init();
motor_1.init();
motor_0.initFOC();
motor_1.initFOC();
On the first flash, initFOC() does not require any parameters. After powering on, SimpleFOC will run a self-test to detect the motor's rotation direction and electrical angle. The results will be printed to the serial port. After obtaining the parameters from the serial output, fill them into initFOC:
motor_0.init();
motor_1.init();
motor_0.initFOC(4.53, Direction::CW);
motor_1.initFOC(0.64, Direction::CCW);
As shown above, each user's parameters will be different. Flash the code a second time. After this, the startup process will no longer consume time on self-testing.
Mobile Control
Install the Dian Deng app on your phone: https://diandeng.tech/dev (opens in a new tab) Create a Bluetooth device and add a joystick control named "JoyKey".
Image: Insert image description here (opens in a new tab) Image: Insert image description here (opens in a new tab)
For remote control of lighting effects, you can develop your own solutions.
Hardware Analysis and Conceptual Design
The circuit is divided into one main board, two encoder daughterboards, and two RGB light boards.
- The main board integrates a brushless motor driver, control section, MPU6050 sensor, power supply, serial port, and various interfaces.
- The driver board carries an AS5600 magnetic encoder and communicates with the main board via I²C.
- The main board provides 5 V power and control signals to the RGB boards.
The motors are 2210 brushless gimbal motors with 7 pole pairs.
Brushless Motor Drive Section
The drive is the core component of the car and the key to making the car move. When the brushless motor is running, the three-phase voltage of the motor needs to be controlled separately. When using the FOC algorithm for control, a position sensor is also required to provide the relative position of the rotor and stator. If torque control is desired, a current sensor is also needed. When I first learned about FOC, I adopted the open-source FOC drive solution from Light Brother, Light Brother's GitHub repository (opens in a new tab).
The motor driver uses the L6234P three-phase H-bridge integrated driver chip.
Image: 在这里插入图片描述 (opens in a new tab)
Current sensing uses the INA240A2 chip with a gain of 50. This chip is also quite expensive and can be replaced with the INA240A1, which has a gain of 20. I used the latter when soldering, and it works just as well.
Image: 在这里插入图片描述 (opens in a new tab)
Motor position detection uses the AS5600 absolute magnetic encoder chip with IIC communication. I designed a small board for this chip and attached it to the motor shaft, connecting it to the main board. A radial magnet also needs to be fixed along the motor shaft axis.
Image: 在这里插入图片描述 (opens in a new tab) Image: 在这里插入图片描述 (opens in a new tab)
Control Section
From the main functions, it is clear that the MCU needs to have Bluetooth and Wi-Fi capabilities, sufficient performance for FOC algorithms, and the ability to control RGB. The recently seen ESP32 seems to be a perfect fit. The ESP32 has 320K RAM, 4M ROM, two cores, with one core dedicated to running the FOC algorithm and the other core running other programs, including RGB control. The SimpleFOC official website states that the ESP32 can perform a single FOC calculation in 1ms, and it also has enough pins to meet the requirements of balanced control.
Sensor Section
Balancing control requires detecting the car's own posture through sensors. I used the most common MPU6050 6-axis accelerometer gyroscope chip.
Image: 在这里插入图片描述 (opens in a new tab)
The Wi-Fi video transmission is implemented by the mainboard connecting the ESP-CAM module via a 2.54mm pin header.
Power Supply Section
- The battery used is a 3S 900mAh RC model battery, providing 12V voltage to the motors.
- The TPS5430 switching chip converts 12V to 5V, supplying power to the RGB and ESP-CAM modules.
- The AMS1117 regulates the 5V down to 3.3V.
Image: Power supply circuit diagram (opens in a new tab) Image: Power distribution diagram (opens in a new tab)
RGB Section
The RGB section uses WS1802B LED lights, and a single IO port of the MCU can drive a string of RGB LEDs.
Image: Insert image description here (opens in a new tab) Image: Insert image description here (opens in a new tab)
Others
I won't go into too much detail about other interfaces and USB-to-serial chips here. You can check out my open-source circuit engineering for more information.
Theoretical Foundation
The theoretical aspects of the brushless motor balancing car require understanding of the following:
- Field-Oriented Control (FOC) algorithm for brushless motors
- Modeling and control algorithms for balancing cars
There are many online resources explaining these theories in a clear and understandable manner. Below are some recommended articles for readers who wish to delve deeper:
Field-Oriented Control (FOC) Algorithm
The FOC algorithm is quite extensive. Beginners can refer to this article by 稚晖君, which is relatively easy to understand:
The official documentation of SimpleFOC also provides a detailed introduction, including source code explanations. Since the code in this project uses the SimpleFOC library, it is recommended to visit the official website to learn about its usage: https://simplefoc.com/ (opens in a new tab)
Other useful articles:
- BLDC Motor Control Algorithm — Overview of FOC (opens in a new tab)
- FOC Learning Notes 1 — Preparation (opens in a new tab)
Self-Balancing Car Modeling and Control Algorithm
- For the modeling part of self-balancing cars, the reference solution by Tsinghua University's Zhu Qing provides a very clear introduction to the mathematical modeling of self-balancing cars. Those who need to understand the self-balancing car model can refer to it multiple times.
- For tuning the balance loop and speed loop PID of a self-balancing car, you can read this article: PID Tuning for Self-Balancing Cars — This is How It Should Be Done!!! (opens in a new tab)
Software Section
The ESP32 microcontroller has two cores, and I use FreeRTOS to schedule them. The overall software framework is based on Arduino + FreeRTOS.
Required Libraries
Adafruit_NeoPixel (opens in a new tab) Used to drive the RGB LED strip blinker-library (opens in a new tab) Bluetooth library from Blinker Technology for communication between the balancing car and a mobile app via Bluetooth MPU6050_light (opens in a new tab) MPU6050 driver library Arduino-FOC (opens in a new tab) Arduino support library for SimpleFOC
Core 0: Task 0: SimpleFOC current loop calculations, pitch angle data reading, and motor voltage output
Core 1: Task 1: Balance loop and speed loop PID calculations Task 2: RGB flashing and receiving control commands from the phone via Bluetooth
TASK 0
The code for this task is relatively simple. loopFOC and move are responsible for the current loop calculation and voltage output of the SimpleFOC.
mpu.update() reads MPU6050 data and returns the pitch angle. The pitch angle can be obtained through complementary filtering.
shaftVelocity returns the motor speed, which is used for the PID calculation of the speed loop in Task 1.
void task_motor_move(void *pvParameters)
{
while (1)
{
// iterative setting FOC phase voltage
motor_1.loopFOC();
motor_0.loopFOC();
// Serial.printf("d:%f,%f\n",motor_1.shaftVelocity(),motor_0.shaftVelocity());
motor_1.move(motor_output_left);
motor_0.move(motor_output_right);
// // iterative function setting the outter loop target
mpu.update();
speed_right = motor_0.shaftVelocity();
speed_left = motor_1.shaftVelocity();
}
}
TASK 1
This task primarily involves PID calculations for the balance loop and speed loop. Using a state machine approach, the balance car's state is divided into three states: STOP, RUNNING, and PICKUP. This is mainly to prevent the motors from spinning wildly when the balance car is picked up.
Upon startup, the state is STOP. When the car is held upright near the balance position, it enters the RUNNING state. If the tilt angle becomes too large after picking up the car, it enters the PICKUP state. After rotating the car body counterclockwise, it returns to the STOP state.
In the RUNNING state, the balance loop, speed loop, and angle loop are executed. The balance loop uses PD control, the speed loop uses PI control, and the angle loop uses only a single P controller.
void task_control(void *pvParameters)
{
while (1)
{
angle_pitch = mpu.getAngleY();
// Serial.printf("d:%f\n",angle_pitch);
acc_Z = mpu.getAccZ();
if (running_state == STATE_STOP)
{
if (abs(angle_pitch - angle_pitch_offset) < 0.5)
{
running_state = STATE_RUNNING;
}
}
else if (running_state == STATE_RUNNING)
{
speed_average = (speed_right + speed_left) / 2;
if (abs(angle_pitch - angle_pitch_offset) > 50 )
{
speed_I_sum=0;//Clear integral coefficient
running_state = STATE_PICKUP;
}
else
{
speed_target = translate_speed;
speed_error = speed_target - speed_average;
speed_I_sum+=(speed_error*speed_I);
if(speed_I_sum>15){
speed_I_sum = 15;
}
if(speed_I_sum<-15){
speed_I_sum = -15;
}
angle_target_pitch = speed_P * speed_error + speed_I_sum;
motor_output_left = angle_P * ((angle_pitch - angle_pitch_offset) - angle_target_pitch) + angle_D * mpu.getGyroY()+rotate_speed; //+ angle_D * mpu.getGyroY()
motor_output_right = angle_P * ((angle_pitch - angle_pitch_offset) - angle_target_pitch) + angle_D * mpu.getGyroY()-rotate_speed;
angle_last_pitch = angle_pitch;
}
}
else if (running_state == STATE_PICKUP)
{
motor_output_left = 0;
motor_output_right = 0;
if (speed_right > 3.14 && speed_left < -3.14)
{
running_state = STATE_STOP;
}
}
vTaskDelay(1);
}
}
TASK 2
Task 2 is used for RGB effect display and receiving data sent via Bluetooth from the Blinker app on a mobile phone. Most RGB effects include a delay function, and conventional delay functions simply wait in place, which significantly reduces CPU efficiency and affects other programs. Here, I have written a series of non-blocking RGB effects: after displaying the effect, instead of waiting in place, the program directly switches to other tasks for scheduling. For details, please refer to my code.
void task_neopixel(void *pvParameters)
{
uint32_t neopixel_pattern_previous = 0;
uint32_t neopixel_pattern_interval = 500;
uint32_t neopixel_pattern_number = 0;
while (1)
{
// Effect switching
if((neopixel_count-neopixel_pattern_previous)>neopixel_pattern_interval){
neopixel_pattern_previous = neopixel_count;
neopixel_pattern_number++;
neopixel_state_clear(strip_1_handler);
neopixel_state_clear(strip_2_handler);
if(neopixel_pattern_number>4){
neopixel_pattern_number = 0;
}
}
switch(neopixel_pattern_number){
case 0:
neopixel_police_style_2(strip_2_handler,10,5);
neopixel_police_style_2(strip_1_handler,10,5);
break;
case 1:
neopixel_police_style_1(strip_2_handler,10,5);
neopixel_police_style_1(strip_1_handler,10,5);
break;
case 2:
neopixel_loop(strip_2_handler, 255, 255, 255, 4, 4);
neopixel_rainbow(strip_1_handler);
break;
case 3:
neopixel_bounce(strip_1_handler, 0, 255, 0, 2, 3, 0);
neopixel_fade_in_out(strip_2_handler, 255, 0, 255, 2, 1,25);
break;
case 4:
neopixel_spread_out(strip_2_handler, strip_2_handler.strip.Color(0,255,0), strip_2_handler.strip.Color(0,0,255), 4, 10);
neopixel_spread_out(strip_1_handler, strip_1_handler.strip.Color(255,0,0), strip_1_handler.strip.Color(255,255,0), 4, 10);
break;
default:
break;
}
neopixel_count++;
#ifdef USE_BLINKER
Blinker.run();
#endif
vTaskDelay(10);
}
}
Files by @FranHawk, at commit 4fd3b23, read from GitHub in your browser. Hardware license: MIT; software license: MIT. Robotheca doesn't review code: read it before running it.