Đôi nét về Robot 2 bánh tự cân bằng

Robot 2 bánh tự cân bằng dựa trên mô hình con lắc ngược là một đối tượng phi tuyến với các
tham số bất định khó điều khiển với 6 biến trạng thái. Đặc điểm nổi bật của Robot 2 bánh tự cân
bằng là cơ chế tự cân bằng, giúp cho xe dù chỉ có một trục chuyển động với hai bánh nhưng luôn
ở trạng thái cân bằng. Có rất nhiều công trình nghiên cứu về xe hai bánh tự cân bằng, nghiên cứu
điều khiển xe 2 bánh tự cân bằng dùng giải thuật cuốn chiếu (backstepping control), H vô cùng,
LQR, phương pháp điều khiển bền vững, cho thấy khả năng thích nghi và hiệu quả của những
giải pháp điều khiển.

Linh kiện trong bài viết

1. Arduino Pro Mini

2. Module GY-521 với MPU-6050

3. Module DRV8833

4. Module 5V boost converter (Nguồn boost lên 5V, 2A)

5. Cảm biến siêu âm US-020 (Bạn cũng có thể dùng module HC-04, HC-05)

6. Pin NCR18650 hoặc các loại pin 18650

7. Động cơ giảm tốc N20, 6V, 200 r

8. Các phụ kiện gá lắp khác

Lắp đặt thiết bị
Nguyên tắc con lắc ngược (inverted pendulum)

Nó giống như việc giữ thăng bằng một cây gậy trên ngón tay. Để giữ thăng bằng, chúng ta phải
di chuyển ngón tay của mình theo hướng nghiêng và tốc độ nghiêng của cây gậy. Để điều khiển
động cơ bánh xe cho robot tự cân bằng qua mạch cầu L298N, chúng ta cần một số thông tin về
trạng thái của robot như: điểm thăng bằng cần cài đặt cho robot, hướng mà robot đang nghiêng,
góc nghiêng và tốc độ nghiêng. Tất cả các thông tin này được thu thập từ MPU6050 và đưa vào
bộ điều khiển PID để tạo ra một tín hiệu điều khiển động cơ, giữ cho robot ở điểm thăng bằng.
Những gì chúng ta đang cố gắng làm ở đây là giữ cho trọng tâm của robot vuông góc với mặt
đất.

Đo góc nghiêng bằng Accelerometer của MPU6050

MPU6050 có gia tốc kế 3 trục và con quay hồi chuyển 3 trục. Gia tốc kế đo gia tốc dọc theo ba
trục và con quay hồi chuyển đo tốc độ góc ba trục. Để đo góc nghiêng của robot, chúng ta cần
các giá trị gia tốc dọc theo trục y và z Hàm atan2 (y, z) cho biết góc có đơn vị là radian giữa trục
z dương của mặt phẳng và điểm được cho bởi tọa độ (z, y) trên mặt phẳng đó, với dấu dương cho
góc ngược chiều kim đồng hồ (nửa mặt phẳng phải), y> 0) và dấu âm cho các góc theo chiều kim
đồng hồ (nửa mặt phẳng trái, y <0). Chúng ta sử dụng thư viện được viết bởi Jeff Rowberg để
đọc dữ liệu từ MPU6050.

Ví dụ chương trình đọc góc nghiêng:

#include “Wire.h”
#include “I2Cdev.h”

#include “MPU6050.h”

#include “math.h”
MPU6050 mpu;
int16_t accY, accZ;

float accAngle;
void setup()
{
mpu.initialize();
Serial.begin(9600);
}
void loop() {
accZ = mpu.getAccelerationZ();
accY = mpu.getAccelerationY();
accAngle = atan2(accY, accZ)*RAD_TO_DEG;
if(isnan(accAngle));
else
Serial.println(accAngle);
}

Đo góc nghiêng bằng Gyroscope của MPU6050

Con quay hồi chuyển 3 trục của MPU6050 đo tốc độ góc (vận tốc quay) dọc theo ba trục. Đối
với robot tự cân bằng, vận tốc góc dọc theo trục x là đủ để đo tốc độ ngã của robot. Trong code
được đưa ra dưới đây, chúng ta đọc giá trị con quay hồi chuyển về trục x, chuyển đổi nó thành
độ/ giây và sau đó nhân nó với thời gian vòng lặp để có được sự thay đổi về góc. Chúng ta cộng
nó vào góc trước để có được góc hiện tại.
Ví dụ:

#include “Wire.h”
#include “I2Cdev.h”
#include “MPU6050.h”

MPU6050 mpu;
int16_t gyroX, gyroRate;
float gyroAngle=0;
unsigned long currTime, prevTime=0, loopTime;
void setup() {
mpu.initialize();
Serial.begin(9600);
}
void loop() {
currTime = millis();
loopTime = currTime – prevTime;
prevTime = currTime;
gyroX = mpu.getRotationX();
gyroRate = map(gyroX, -32768, 32767, -250, 250);
gyroAngle = gyroAngle + (float)gyroRate*loopTime/1000;
Serial.println(gyroAngle);
}

Vị trí của MPU6050 khi chương trình bắt đầu chạy là điểm nghiêng bằng không. Góc nghiêng sẽ
được đo đối với điểm này. Giữ cho robot ổn định ở một góc cố định và bạn sẽ thấy rằng góc sẽ
tăng hoặc giảm dần. Nó sẽ không ổn định. Điều này là do sự di chuyển vốn có của con quay hồi
chuyển.Trong đoạn code trên, thời gian vòng lặp được tính bằng hàm millis () được tích hợp vào
IDE Arduino. Trong các bước sau, chúng ta sẽ sử dụng ngắt bộ hẹn giờ để tạo khoảng thời gian
lấy mẫu chính xác. Khoảng thời gian lấy mẫu này cũng sẽ được sử dụng để tạo ra đầu ra bằng bộ
điều khiển PID.

Kết hợp các kết quả bằng bộ lọc bổ sung

Chúng ta có hai phép đo góc từ hai nguồn khác nhau. Phép đo từ gia tốc bị ảnh hưởng bởi
chuyển động ngang đột ngột và phép đo từ con quay hồi chuyển dần dần sai nhiều so với giá trị
thực. Nói cách khác, việc đọc gia tốc bị ảnh hưởng bởi tín hiệu thời gian ngắn và con quay hồi
chuyển đọc bằng tín hiệu thời gian dài. Những số liệu này, theo cách nào đó, bổ sung cho
nhau. Kết hợp cả hai bằng cách sử dụng bộ lọc bổ sung và chúng ta nhận được phép đo góc ổn
định, chính xác. Bộ lọc bổ sung về cơ bản là một bộ lọc thông cao tác động lên con quay hồi
chuyển và bộ lọc thông thấp tác động lên gia tốc kế để lọc giá trị và nhiễu từ phép đo.

currentAngle = 0.9934 * (previousAngle + gyroAngle) + 0.0066 * (accAngle)

0.9934 và 0.0066 là hệ số lọc cho hằng số thời gian lọc là 0,75 giây. Bộ lọc thông thấp cho phép
bất kỳ tín hiệu nào dài hơn khoảng thời gian này có thể đi qua nó và bộ lọc thông cao cho phép
bất kỳ tín hiệu nào ngắn hơn khoảng thời gian này đi qua. Phản ứng của bộ lọc có thể được tinh
chỉnh bằng cách chọn hằng số thời gian chính xác. Giảm hằng số thời gian sẽ cho phép tăng gần
với giá trị chính xác.

Loại bỏ lỗi bù tốc độ và con quay hồi chuyển

Tải xuống và chạy code được đưa ra trong trang này để hiệu chỉnh bù trừ của MPU6050. Bất kỳ
lỗi nào do offset có thể được loại bỏ bằng cách xác định các giá trị offset trong hàm setup() như
hình dưới đây.
mpu.setYAccelOffset(1593);
mpu.setZAccelOffset(963);
mpu.setXGyroOffset(40);

Điều khiển PID để tạo đầu ra

PID là viết tắt của Proportional, Integral và Derivative. Mỗi thuật ngữ này cung cấp một hành
động cho robot tự cân bằng.

Proportional tạo ra một phản ứng là tỷ lệ thuận với lỗi. Đối với hệ thống của chúng ta, lỗi là góc
nghiêng của robot.
Integral tạo ra một phản ứng dựa trên các lỗi tích lũy. Về cơ bản, đây là tổng của tất cả các lỗi
nhân với khoảng thời gian lấy mẫu. Đây là một phản ứng dựa trên hành vi của hệ thống trong
quá khứ.
Derivative tỷ lệ với đạo hàm của lỗi. Đây là sự khác biệt giữa lỗi hiện tại và lỗi trước đó chia
cho khoảng thời gian lấy mẫu. Điều này đóng vai trò như một thuật ngữ dự đoán giúp cách robot
có thể hoạt động trong vòng lặp lấy mẫu tiếp theo.

Nhân mỗi thuật ngữ với các hằng số tương ứng của chúng (ví dụ, Kp, Ki và Kd) và lấy tổng kết
quả, chúng ta tạo ra đầu ra được gửi như lệnh để điều khiển động cơ.

Điều chỉnh hằng số PID

1. Đặt Ki và Kd về 0 và tăng dần Kp sao cho robot bắt đầu dao động về vị trí 0.
2. Tăng Ki để phản xạ của robot nhanh hơn khi nó mất cân bằng. Ki phải đủ lớn sao cho góc
nghiêng không tăng. Robot sẽ quay trở lại vị trí 0 nếu nó nghiêng.
3. Tăng Kd để giảm dao động. Các overshoots cũng nên được giảm.
4. Lặp lại các bước trên bằng cách tinh chỉnh từng thông số để đạt được kết quả tốt nhất.

Thêm cảm biến khoảng cách

Cảm biến khoảng cách siêu âm sử dụng trong dự án này là US-020. Nó có bốn chân là Vcc, Trig,
Echo và Gnd. Nó được cấp nguồn 5V. Các trigger và các chân echo tương ứng với các chân số 9
và 8 của Arduino. Chúng ta sẽ sử dụng thư viện NewPing để lấy giá trị khoảng cách từ cảm
biến. Chúng ta sẽ đọc khoảng cách sau mỗi 100 mili giây và nếu giá trị nằm trong khoảng từ 0
đến 20cm, chúng ta sẽ điều khiển rô bốt để thực hiện xoay vòng. Điều này là để điều khiển robot
tránh xa chướng ngại vật.

Code chương trình

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
#include <NewPing.h>

#define leftMotorPWMPin 6
#define leftMotorDirPin 7
#define rightMotorPWMPin 5
#define rightMotorDirPin 4

#define TRIGGER_PIN 9
#define ECHO_PIN 8
#define MAX_DISTANCE 75

#define Kp 40
#define Kd 0.05
#define Ki 40
#define sampleTime 0.005
#define targetAngle -2.5

MPU6050 mpu;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

int16_t accY, accZ, gyroX;

volatile int motorPower, gyroRate;
volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0;
volatile byte count=0;
int distanceCm;

void setMotors(int leftMotorSpeed, int rightMotorSpeed) {

if(leftMotorSpeed >= 0) {
analogWrite(leftMotorPWMPin, leftMotorSpeed);
digitalWrite(leftMotorDirPin, LOW);
}
else {
analogWrite(leftMotorPWMPin, 255 + leftMotorSpeed);
digitalWrite(leftMotorDirPin, HIGH);
}
if(rightMotorSpeed >= 0) {
analogWrite(rightMotorPWMPin, rightMotorSpeed);
digitalWrite(rightMotorDirPin, LOW);
}
else {
analogWrite(rightMotorPWMPin, 255 + rightMotorSpeed);
digitalWrite(rightMotorDirPin, HIGH);
}
}

void init_PID() {
// initialize Timer1
cli(); // disable global interrupts
TCCR1A = 0; // set entire TCCR1A register to 0
TCCR1B = 0; // same for TCCR1B
// set compare match register to set sample time 5ms
OCR1A = 9999;
// turn on CTC mode
TCCR1B |= (1 << WGM12);
 // Set CS11 bit for prescaling by 8
TCCR1B |= (1 << CS11);
// enable timer compare interrupt
TIMSK1 |= (1 << OCIE1A);
sei(); // enable global interrupts
}

void setup() {
// set the motor control and PWM pins to output mode
pinMode(leftMotorPWMPin, OUTPUT);
pinMode(leftMotorDirPin, OUTPUT);
pinMode(rightMotorPWMPin, OUTPUT);
pinMode(rightMotorDirPin, OUTPUT);
// set the status LED to output mode
pinMode(13, OUTPUT);
// initialize the MPU6050 and set offset values
mpu.initialize();
mpu.setYAccelOffset(1593);
mpu.setZAccelOffset(963);
mpu.setXGyroOffset(40);
// initialize PID sampling loop
init_PID();
}

void loop() {
// read acceleration and gyroscope values
accY = mpu.getAccelerationY();
accZ = mpu.getAccelerationZ();
gyroX = mpu.getRotationX();
// set motor power after constraining it
motorPower = constrain(motorPower, -255, 255);
setMotors(motorPower, motorPower);
// measure distance every 100 milliseconds
if((count%20) == 0){
distanceCm = sonar.ping_cm();
}
if((distanceCm < 20) && (distanceCm != 0)) {
setMotors(-motorPower, motorPower);
}
}
// The ISR will be called every 5 milliseconds
ISR(TIMER1_COMPA_vect)
{
// calculate the angle of inclination
accAngle = atan2(accY, accZ)*RAD_TO_DEG;
gyroRate = map(gyroX, -32768, 32767, -250, 250);
 gyroAngle = (float)gyroRate*sampleTime;
currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);

error = currentAngle - targetAngle;

errorSum = errorSum + error;
errorSum = constrain(errorSum, -300, 300);
//calculate output from P, I and D values
motorPower = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-
prevAngle)/sampleTime;
prevAngle = currentAngle;
// toggle the led on pin13 every second
count++;
if(count == 200) {
count = 0;
digitalWrite(13, !digitalRead(13));
}
}

Các dự án Robot 2 bánh cân bằng khác

Electric DIY Lab

Kết nối phần cứng

Board ArduinoMega Chức

Kết nối Ghi chú
2560 năng
Chân 2 Input Encode motor
Chân 3 Input Encode motor
Chân 4 Output Chân input L298N – IN1 Output L298N nối động cơ
Chân 5 Output Chân input L298N – IN2
Chân 6 Output Chân input L298N – IN3
Chân 7 Output Chân input L298N – IN4
Chân 9 Output Chân EA – L298N
Chân 10 Output Chân EB – L298N
Chân SDA cảm biến Giao tiếp chuẩn I2C,
Chân 20 Input
Gyro MPU6050
Chân SCL cảm biến
Chân 21 Input
Gyro

Code chương trình:

#include <Kalman.h>
#include<Servo.h>
#include<Wire.h>
#include<I2Cdev.h>
#include<MPU6050.h>
MPU6050 CBgoc;
Kalman kalmanX;
//IMU 6050====================================================
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;
float accXangle;
float gyroXangel;
float kalAngelX;
unsigned long timer;
uint8_t i2cData[14];
float CurrentAngle;
// MOTOR====================================================
int AIN1 = 4;
int AIN2 = 5;
int BIN1 = 6;
int BIN2 = 7;
int CIN1 = 9;
int CIN2 = 10;
int speed;
// PID====================================================
const float Kp = 30;
const float Ki = 1;
const float Kd = 8;
float pTerm, iTerm, dTerm, integrated_error, last_error, error;
const float K = 1.9*1.12;
#define GUARD_GAIN 10.0
#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) >
(t);_lasttime += (t))

void setup()
{
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
Serial.begin(9600);
Wire.begin();

i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz

i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8
KHz sampling
i2cData[2] = 0x00;
i2cData[3] = 0x00;
while(i2cWrite(0x19,i2cData,4,false));
while(i2cWrite(0x6B,0x01,true));
while(i2cRead(0x75,i2cData,1));
if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while(1);
}
delay(100);

//Kalman====================================================
while(i2cRead(0x3B,i2cData,6));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
kalmanX.setAngle(accXangle);
gyroXangel = accXangle;
timer = micros();
}

void loop()
{
Serial.println(accX);
// Serial.println(accY);
// Serial.println(accZ);
Serial.println(accXangle);
Serial.println(CurrentAngle);
runEvery(25)
{
dof();
if(CurrentAngle <=179 && CurrentAngle >=178.5)
{
stop();
}
else
 {
if(CurrentAngle < 230 && CurrentAngle >130)
{
Pid();
Motors();
}
else
{
stop();
}
}
}
}
void Motors()
{
if(speed > 0)
{
analogWrite(CIN1, speed);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
analogWrite(CIN2, speed);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
}
else
{
speed = map(speed,0,-255,0,255);
analogWrite(CIN1, speed);
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
analogWrite(CIN2, speed);
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
}
}
void stop()
{
speed = map(speed,0,-150,0,150);
analogWrite(CIN1, speed);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
analogWrite(CIN2, speed);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
}
void Pid()
{
error = 180 - CurrentAngle; // 180 = level
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki*constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd*(error - last_error);
last_error = error;
speed = constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}
void dof()
{
while(i2cRead(0x3B,i2cData,14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = ((i2cData[6] << 8) | i2cData[7]);
gyroX = ((i2cData[8] << 8) | i2cData[9]);
gyroY = ((i2cData[10] << 8) | i2cData[11]);
gyroZ = ((i2cData[12] << 8) | i2cData[13]);
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
double gyroXrate = (double)gyroX/131.0;
CurrentAngle = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000);
timer = micros();
}

Cách giao tiếp với Module cảm biến góc gia tốc MPU6050 GY-521

Chào các bạn!

Hôm nay LinhKien69 xin chia sẻ với các bạn cách thức giao tiếp giữa vi điều khiển với cảm biến
góc gia tốc MPU6050 GY-521
MPU-6050 tích hợp 6 trục cảm biến bao gồm:

+ con quay hồi chuyển 3 trục (3-axis MEMS gyroscope)

+ cảm biến gia tốc 3 chiều (3-axis MEMS accelerometer)

Ngoài ra, MPU-6050 còn có 1 đơn vị tăng tốc phần cứng chuyên xử lý tín hiệu (Digital Motion
Processor – DSP) do cảm biến thu thập và thực hiện các tính toán cần thiết. Điều này giúp giảm
bớt đáng kể phần xử lý tính toán của vi điều khiển, cải thiện tốc độ xử lý và cho ra phản hồi
nhanh hơn. Đây chính là 1 điểm khác biệt đáng kể của MPU-6050 so với các cảm biến gia tốc và
gyro khác.
MPU-6050 có thể kết hợp với cảm biến từ trường (bên ngoài) để tạo thành bộ cảm biến 9 góc
đầy đủ thông qua giao tiếp I2C.
Các cảm biến bên trong MPU-6050 sử dụng bộ chuyển đổi tương tự – số (Anolog to Digital
Converter – ADC) 16-bit cho ra kết quả chi tiết về góc quay, tọa độ… Với 16-bit bạn sẽ có 2^16
= 65536 giá trị cho 1 cảm biến.
Tùy thuộc vào yêu cầu của bạn, cảm biến MPU-6050 có thể hoạt động ở chế độ tốc độ xử lý cao
hoặc chế độ đo góc quay chính xác (chậm hơn). MPU-6050 có khả năng đo ở phạm vi:
+ con quay hồi chuyển: ± 250 500 1000 2000 dps

+ gia tốc: ± 2 ± 4 ± 8 ± 16g

Hơn nữa, MPU-6050 có sẵn bộ đệm dữ liệu 1024 byte cho phép vi điều khiển phát lệnh cho cảm
biến, và nhận về dữ liệu sau khi MPU-6050 tính toán xong.

– Cách lấy dữ liệu từ cảm biến.

MPU6050 chỉ hỗ trợ chuẩn giao tiếp I2C để xuất giá trị đo sang thiết bị khác (Master) hoặc chính
nó lại là nơi xử lý, lưu trữ tín hiệu từ các thiết bị khác kết nối vào.
Đầu tiên ta cần nắm được Protocol của chuẩn I2C

Slaver Address của MPU6050 mặc định là 0x68 = 0b1101000

Quá trình truyền hoặc nhận dữ liệu được bắt đầu khi có tín hiệu “Start” và kết thúc bởi tín hiệu
“Stop

i2c_start();

i2c_stop();
Một khi đã có điều kiện “start” thì Master phải truyền địa chỉ của thanh ghi cần tác động và hành
động tương ứng(read/write )
Đối với MPU6050 quy luật truyền do nhà sản xuất đưa ra:
Quá trình đưa dữ liệu từ vi điều khiển xuống cảm biến (chính là việc cài đặt cấu hình cho cảm
biến) Write:

Hàm ghi dữ liệu đến một thanh ghi của MPU6050.

void MPU6050_I2C_ByteWrite(u8* pBuffer, u8 writeAddr)

{
//Gửi bit start
I2C_GenerateSTART(MPU6050_I2C, ENABLE);
while(!I2C_CheckEvent(MPU6050_I2C,I2C_EVENT_MASTER_MODE_SELECT));
//Gửi địa chỉ của MPU6050
I2C_Send7bitAddress(MPU6050_I2C, MPU6050_ADDR,I2C_Direction_Transmitter);
while(!I2C_CheckEvent(MPU6050_I2C,
I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
//Truy cập đến 1 thanh ghi MPU6050
I2C_SendData(MPU6050_I2C, writeAddr);
 while(!I2C_CheckEvent(MPU6050_I2C,
I2C_EVENT_MASTER_BYTE_TRANSMITTED));
//Gửi dữ liệu đến thanh ghi vừa truy cập
I2C_SendData(MPU6050_I2C, *pBuffer);
/* Test on EV8 and clear it */
while(!I2C_CheckEvent(MPU6050_I2C,
I2C_EVENT_MASTER_BYTE_TRANSMITTED));
//Gửi bit STOP
I2C_GenerateSTOP(MPU6050_I2C, ENABLE);
}

Hàm đọc dữ liệu từ một thanh ghi của MPU6050 rồi ghi vào mảng pBuffer

void MPU6050_I2C_BufferRead(uint8_t* pBuffer, u8 readAddr, u16 NumByteToRead)

Hàm cấu hình cho MPU6050.

void MPU6050_init()
{
w_data[0]=0x80;
MPU6050_I2C_ByteWrite(w_data, PWR_MGMT_1); //Reset Cam Bien
delay_us(5);
w_data[0]=0x00;
MPU6050_I2C_ByteWrite(w_data, PWR_MGMT_1);// Cam bien hoat dong
delay_us(5);
w_data[0]=0x00;
MPU6050_I2C_ByteWrite(w_data, SMPLRT_DIV);
delay_us(5);
w_data[0]=0x07;
MPU6050_I2C_ByteWrite(w_data, CONFIG_MPU);
delay_us(5);
w_data[0]=0x18;
MPU6050_I2C_ByteWrite(w_data, GYRO_CONFIG); // Do phan giai GYRO la +-2000 do/s
delay_us(5);
w_data[0]=0x10;
MPU6050_I2C_ByteWrite(w_data, ACCEL_CONFIG);
delay_us(5);
}

Hàm void MPU6050_get_gyro_acc(int16_t* AccelGyro) lưu lại dữ liệu vào mảng mới theo đúng
giá trị bank cao, bank thấp

Hàm void MPU6050_get_value() lấy mẫu cho các giá trị góc đọc được