Xe hai bánh tự cân bằng điều khiển thông qua Blynk

Một đề tài cũ mà mới đó chính là “Xe hai bánh tự cân bằng”, chắc hẳn các bạn đã từng nghe nó ở đâu đó đúng không? Mình từng làm chiếc xe này từ hồi sinh viên năm 3, năm 2013, tính ra cũng được 8 năm rồi đấy. Hồi đấy cũng từng cúp học để ăn nằm với nó mấy tháng liền chỉ đề tìm hệ số PID. Và cuối cùng, sau chừng đó thời gian, mình xem lại code và nhận ra rằng mình thiết kế từ cơ khí đến code nó sai bét nhè, may mà vẫn qua được đồ án :))). Nên bây giờ cùng làm để báo thù nó nào!

Chuẩn bị phụ tùng

Mình lần này làm xe cân bằng với tiêu chí nhỏ gọn nên sẽ lựa chọn những phụ tùng mini hết sức có thể. Linh kiện bao gồm:

Module để lập trình điều khiển ESP8266 nodeMCU x1

Các bạn nên tháo các chân hàn của module này ra nhé, sau đó sẽ hàn dây cần thiết lên các pin thì mạch sẽ nhỏ gọn hơn.

Cảm biến góc MPU6050 x1

Động cơ giảm tốc mini GA12 N20 + gá động cơ 200rpm x2

Mình khuyên các bạn nên mua loại 12V 500rpm trở xuống, vì motor này khá yếu so với trọng lượng của tất cả linh kiện.

Bánh xe 43mm Ga12

Pin 18650 và khay pin nối tiếp x2

Các bạn nên mua loại pin tốt dòng xả cao nhé (10C). Mình từng làm pin hơi dổm nên lúc chạy driver điều khiển motor khá nóng.

Driver Motor L298 mini

Loại driver này hơi khác so với L298N, nó chỉ điều khiển motor qua 2 chân A B, muốn quay thuận thì A xuất xung, B xuất 0, quay ngược thì A xuất 0 còn B xuất xung. Ngoài ra còn có thêm chế độ hãm và standby khi cả 2 chân A B cùng trạng thái High hoặc Low.

Thiết kế cơ khí

Show nhẹ thiết kế của mình cho mọi người xem nhé :D. Nhìn gọn gàng vậy chứ lúc đi dây là thay đổi nhiều thứ lắm. Nhưng cơ bản là nó “đẹp” như vậy :))).

Lưu ý to bự cho các bạn: Đầu tiên là khi thiết kế cần đặt đúng tâm của cảm biến MPU6050 nằm trên trục ngang nối hai trục của motor, làm như vậy để khi xe nghiêng góc trái hay phải thì sai số đều cho cả hai bên. Thứ hai là cần cân đối tải trọng tốt nhất có thể để xe có thể tự cân bằng tĩnh (nghĩa là chưa làm gì mà nó vẫn cân bằng ấy :D).

Đấu nối dây

Kết nối L298 mini

Tại các chân IN1, IN2, IN3, IN4 sẽ kết nối vào các IO của ESP8266. Nhưng mà IO nào nên dùng? Nếu các bạn thành tâm muốn biết, mình sẽ trả lời rằng: “Hãy lên google và tìm ESP8266 pin reference”. Sau một hồi tìm kiếm thì mình đã chọn được các IO: 0, 12, 13, 14 (né các chân I2C ra nhé, nó còn giao tiếp với MPU6050 :D).

Lưu ý: Nguyên nhân mình chỉ dùng 2 viên pin 18650 là vì nguồn vào module này chỉ từ 2 đến 10V thôi nhé. Ai ham hố max công suất xịt khói ráng chịu.

Kết nối MPU6050

Có lẽ đây là sơ đồ kết nối các bạn thường thấy. Chỉ cần kết nối nguồn, SCL và SDA là xong nhưng không phải là tất cả đâu nhé :D. Trong quá trình test đọc góc, mình thấy nhiều lúc cảm biến trả về giá trị lớn hơn nhiều so với giá trị bình thường, dù mình vẫn đang để cảm biến đứng yên. Vì vậy các bạn nên dùng thêm chân INT để nhận biết thời điểm cảm biến sẵn sàng lấy data và thời gian mỗi chu trình lấy. Điều này sẽ làm cho giá trị góc trả về ổn định hơn.

Vì vậy hãy nối chân INT của MPU6050 với một IO của ESP8266 nhé. Mình chọn GPIO15.

Kết nối nguồn

Điện áp của hai viên pin 18650 nối tiếp là 8.5V, các bạn chia ra một nhánh cắm cấp nguồn cho driver động cơ, một nhánh cấp nguồn cho ESP8266 (chân Vin). Lưu ý: Cần thêm một công tắc để ngắt nguồn khi nạp code, vì điện áp cổng USB chỉ có 5V, mình từng để song song 2 nguồn một lúc nhưng vẫn không sao. Tuy nhiên cẩn thận vẫn hơn nha các bạn.

Code

Trước khi lập trình, ta cần phải hiểu nguyên lý của xe cân bằng trước. Khi đã hiểu rồi thì chia nhỏ từng nhiệm vụ để code, sau đó ghép code sao cho pờ rồ tí là nó chạy ấy mà :))).

Hãy tưởng tượng mình đang đứng yên, người thẳng đứng, nếu có một người đẩy mình theo một hướng nhất định thì mình có xu hướng di chuyển về hướng đó để lấy lại trọng tâm cân bằng. Xe cân bằng cũng vậy, nó dựa trên cảm biến MPU6050 để nhận biết được trạng thái góc nghiêng hiện tại của nó. Xem như 0 là góc thẳng đứng mà nó cần duy trì để cân bằng thì đây được gọi là Setpoint. Khi một lực tác động vào xe cân bằng, trạng thái góc sẽ thay đổi, tùy vào âm hay dương để xe biết di chuyển về hướng nào. Tuy nhiên di chuyển với tốc độ như thế nào, vận tốc bao nhiêu để làm cho xe cân bằng nhanh nhất, không bị rung lắc thì bộ PID sẽ đảm nhiệm vai trò này.

Kết luận: Code xe cân bằng sẽ có hai phần chính là đọc giá trị góc cảm biến MPU6050 và điều khiển tốc độ động cơ bằng PID.

Đọc cảm biến MPU6050

Để đọc giá trị góc gửi về từ cảm biến, mình dùng thử viện “MPU6050/MPU6050_6Axis_MotionApps20.h”. Các bạn có thể tải tại đây. Hoặc có thể tải trực tiếp trên Arduino IDE

MPU6050 Electronic Cats

Để đọc giá trị góc chính xác thì ta cần calib cảm biến. Tại Arduino IDE, các bạn chọn Example “IMU_ZERO”. Sau đó để cảm biến ở vị trí cố định, upload code và lấy các giá trị trong serial tương ứng copy vào code của mình ở các dòng sau:

mpu.setXGyroOffset(xxx);

mpu.setYGyroOffset(xxx);

mpu.setZGyroOffset(xxx);

mpu.setZAccelOffset(xxx);

Điều khiển tốc độ động cơ bằng PID

Mình sử dụng thư viện PID_v2 để điều khiển tốc độ động cơ, các bạn có thể tải thư viện này ở Arduino IDE mục Library Manager.

Nói qua một chút về: PID là gì? Các bạn có thể hiểu rằng PID sẽ xuất ra giá trị Output ứng với giá trị Input, sao cho hệ thống đáp ứng ở trạng thái mà các mong muốn (được gọi là Setpoint) một cách nhanh nhất mà không xảy ra các hiện tượng vọt lố, dao động quá lớn.

Đối với trường hợp xe cân bằng: Các bạn muốn xe cân bằng ở vị trí 0º thì đây chính là Setpoint. Xe sẽ đọc các giá trị Input đó chính là giá trị góc mà MPU6050 gửi về, sau đó dựa vào sai số mà chương trình tính toán ra giá trị Output để điều khiển tốc độ động cơ.

Vậy khi sử dụng một bộ điều khiển PID các bạn cần phải lưu ý:

Cần xác định thời gian lấy mẫu

void loop(void)

{

cycle = millis();

mpu_loop();//chương trình chạy xe cân bằng

Blynk.run();//chương trình chạy blynk

Serial.println(millis() - cycle);//xuất ra màn hình thời gian xử lý toàn bộ chương trình để xác định thời gian lấy mẫu

}

Cách điều chỉnh các giá trị Kp, Ki, Kd

Cho tất cả các giá trị Kp, Ki, Kd bằng 0, tăng dần giá trị Kp sao cho xe có thể lắc lư qua về mà vẫn cân bằng được (hoặc các bạn có thể test giá trị Kp sao cho khi tác động lực vào xe về một phía, xe có thể chạy về phía đó nhưng một thời gian ngắn lại bị ngã về hướng ngược lại => cần giảm lại giá trị Kp một ít).

Tăng dần giá trị Kd đến khi nào xe không còn lắc lư qua về nữa. Nhận biết tăng quá lố là khi xe từ trạng thái lắc lư nhiều (Kd nhỏ) sang lắc lư ít (Kd vừa phải) rồi chuyển sang lắc lư nhiều (Kd lớn) lại. Tại giá trị Kd vừa phải, khi tác động lực nhẹ để xe về một phía, xe sẽ di chuyển theo hướng đó. Sau đó xe quay về gần với vị trí ban đầu.

Tăng dần giá trị Ki đến khi nào xe có biểu hiện rung lắc thì dừng lại. Nhận biết bằng cách khi tác động lực nhẹ để xe di chuyển về một phía, xe sẽ nhanh chóng lấy lại được vị trí cân bằng Setpoint mà không xảy ra hiện tượng rung lắc.

Biết giới hạn giá trị Output

ESP8266 khác với Arduino, chân output xuất xung PWM lớn nhất là 1023. Vì vậy Output được giới hạn từ -1023 đến 1023.

Code xe cân bằng

#if defined(ESP8266)

#include <ESP8266WiFi.h>

#else

#include <WiFi.h>

#endif

#include <DNSServer.h>

#include <WiFiClient.h>

#include <WiFiUdp.h>

#include "I2Cdev.h"

#include <ESP8266WiFi.h>

#include <PID_v2.h>

#include <BlynkSimpleEsp8266.h>

#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

#include "Wire.h"

#endif

MPU6050 mpu;

bool dmpReady = false; // set true if DMP init was successful

uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU

uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)

uint16_t packetSize; // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount; // count of all bytes currently in FIFO

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

Quaternion q; // [w, x, y, z] quaternion container

VectorInt16 aa; // [x, y, z] accel sensor measurements

VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements

VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements

VectorFloat gravity; // [x, y, z] gravity vector

float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

#define INTERRUPT_PIN 15 // use pin 15 on ESP8266

char ssid[] = "";

char pass[] = "";

char auth[] = "";

unsigned long cycle = 0;

unsigned long last_current = 0;

unsigned long setdelay = 0;

unsigned long makecycle = 10;

float Kp = 16;

float Ki = 0.2;

float Kd = 0;

float offset = 0;

float last_value = 0;

bool forward = false;

bool initial = false;

double Setpoint, Input, Output, Angle, temp_setpoint, rotate;

PID_v2 myPID(Kp, Ki, Kd, PID::Direct);

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high

void ICACHE_RAM_ATTR dmpDataReady() {

mpuInterrupt = true;

}

void mpu_setup()

{

// join I2C bus (I2Cdev library doesn't do this automatically)

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

Wire.begin();

Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties

#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE

Fastwire::setup(400, true);

#endif

Serial.println(F("Initializing I2C devices..."));

mpu.initialize();

pinMode(INTERRUPT_PIN, INPUT);

Serial.println(F("Testing device connections..."));

Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

Serial.println(F("Initializing DMP..."));

devStatus = mpu.dmpInitialize();

mpu.setXGyroOffset(317);

mpu.setYGyroOffset(-57);

mpu.setZGyroOffset(41);

mpu.setZAccelOffset(1042);

if (devStatus == 0) {

// turn on the DMP, now that it's ready

Serial.println(F("Enabling DMP..."));

mpu.setDMPEnabled(true);

// enable Arduino interrupt detection

Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));

attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);

mpuIntStatus = mpu.getIntStatus();

// set our DMP Ready flag so the main loop() function knows it's okay to use it

Serial.println(F("DMP ready! Waiting for first interrupt..."));

dmpReady = true;

packetSize = mpu.dmpGetFIFOPacketSize();

} else {

Serial.print(F("DMP Initialization failed (code "));

Serial.print(devStatus);

Serial.println(F(")"));

}

BLYNK_WRITE(V6)

{

float pinValue = param.asFloat(); // assigning incoming value from pin V1 to a variable

rotate = pinValue;

}

BLYNK_WRITE(V5)

{

float pinValue = param.asFloat(); // assigning incoming value from pin V1 to a variable

offset = pinValue;

Setpoint = temp_setpoint - offset;

myPID.Start(Input, // input

0, // current output

Setpoint);

Serial.println(Setpoint);

}

BLYNK_WRITE(V1)

{

float pinValue = param.asFloat(); // assigning incoming value from pin V1 to a variable

Kp = pinValue;

myPID.SetTunings(Kp, Ki, Kd);

}

BLYNK_WRITE(V2)

{

float pinValue = param.asFloat(); // assigning incoming value from pin V1 to a variable

Ki = pinValue;

myPID.SetTunings(Kp, Ki, Kd);

}

BLYNK_WRITE(V3)

{

float pinValue = param.asFloat(); // assigning incoming value from pin V1 to a variable

Kd = pinValue;

myPID.SetTunings(Kp, Ki, Kd);

}

BLYNK_WRITE(V4)

{

float pinValue = param.asFloat(); // assigning incoming value from pin V1 to a variable

Setpoint = pinValue;

temp_setpoint = Setpoint;

myPID.Start(Input, // input

0, // current output

Setpoint);

}

void setup(void)

{

pinMode (0, OUTPUT);

pinMode (14, OUTPUT);

pinMode (12, OUTPUT);

pinMode(13, OUTPUT);

analogWrite(14, 1023);

analogWrite(0, 1023);

analogWrite(12, 1023);

analogWrite(13, 1023);

Serial.begin(115200);

Serial.println(F("\nOrientation Sensor OSC output")); Serial.println();

Serial.print(F("WiFi connected! IP address: "));

Serial.println(WiFi.localIP());

Blynk.begin(auth, ssid, pass, "sv.bangthong.com", 8080);

analogWriteRange(1023);

Blynk.syncAll();

last_current = millis();

mpu_setup();

Setpoint = 0;

myPID.Start(Input, // input

0, // current output

Setpoint);

myPID.SetSampleTime(15);

myPID.SetOutputLimits(-1023, 1023);

}

void mpu_loop()

{

if (!dmpReady)

{

return;

}

while (!mpuInterrupt && fifoCount < packetSize) {

if (abs(Input) < 40)

{

Output = myPID.Run(Input);

if (Output <= 0)

{

analogWrite(0, abs(Output));

analogWrite(14, 0);

analogWrite(12, abs(Output));

analogWrite(13, 0);

}

else

{

analogWrite(14, abs(Output));

analogWrite(0, 0);

analogWrite(13, abs(Output));

analogWrite(12, 0);

}

Angle = Input;

}

else

{

Output = 0;

analogWrite(0, abs(Output));

analogWrite(14, 0);

analogWrite(13, abs(Output));

analogWrite(12, 0);

}

mpuInterrupt = false;

mpuIntStatus = mpu.getIntStatus();

fifoCount = mpu.getFIFOCount();

if ((mpuIntStatus & 0x10) || fifoCount == 1024) {

// reset so we can continue cleanly

mpu.resetFIFO();

//Serial.println(F("FIFO overflow!"));

} else if (mpuIntStatus & 0x02) {

while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

mpu.getFIFOBytes(fifoBuffer, packetSize);

fifoCount -= packetSize;

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

Input = ypr[1] * 180 / M_PI;

}

void loop(void)

{

cycle = millis();

mpu_loop();

Blynk.run();

Serial.println(millis() - cycle);//comment nó khi đã xác định được sample time

}

Các bạn cần phải thay đổi code nay tùy thuộc vào trường hợp mà các bạn gặp phải:

Về kiến thức cơ bản về Blynk, các bạn có thể tìm hiểu trên mạng hoặc thông qua các ví dụ để hiểu code của mình.

char ssid[] = “”; //SSID wifi

char pass[] = “”; //Password wifi

char auth[] = “”; //Auth token blynk

myPID.SetSampleTime(15); //Xác định thời gian lấy mẫu là 15ms

myPID.SetOutputLimits(-1023, 1023); //giới hạn Output

Setpoint ; Được xác định bằng cách: đọc giá trị góc của xe khi xe ở vị trí thẳng đứng với mặt bàn.

V1, V2, V3: Setting Kp, Ki, Kd từ Blynk gửi về.

V4: Setting Setpoint.

V5: Được dùng là Joystick trên Blynk để điều khiển xe di chuyển tới và lui. Xe xe tiến tới và lùi khi thay đổi giá trị Setpoint.

Kết luận

Vậy là mình đã hoàn thành xe cân bằng trong một khoảng thời gian ngắn tầm 2 ngày thay vì gần tốn gần hết 2 tháng ăn nằm với nó từ hồi sinh viên. Nhưng mà nhờ 2 tháng vật vã đó mà mình biết khá nhiều ấy: biết sử dụng Solidworks, bắt đầu biết thế nào là lập trình, biết đến đồ điện tử vì nó bốc khói khá nhiều, bắt đầu biết tư duy phân tích vấn đề… Hy vọng bài này sẽ giúp một phần nào đó cho các bạn. Nếu có gì thắc mắc hãy comment, mình sẽ sẵn sàng giải đáp. Cảm ơn các bạn!