Giới thiệu cảm biến Gyro MPU6050 ( CB vận tốc góc) trong Robocon

[aceptoan]

Dưới đây là những gì mình đã sử dụng trong mùa Robocon 2013 và khá ổn. Mong mọi người góp ý thêm vì là lần đầu sử dụng nên có khá nhiều thiếu sót.

-Ý tưởng và lý do sử dụng: Sân Robot năm 2013 như các bạn thấy có rất ít line và hầu hết là line cong nên việc dò đường theo line sẽ làm tốc độ robot giảm đáng kể. (cái này là do phương án chạy, đội mình sử dụng thả từng ring một nên việc chạy theo đường thẳng kẻ theo các ring là nhanh nhất). Nên yêu cầu được đặt ra là Robot chạy thẳng được theo 1 đường giả định. quay chính xác góc mà ít ảnh hưởng đến yếu tố bên ngoài.


-Tại sao là Gyro mà ko phải là la bàn số: theo như các bạn biết thì la bàn số ảnh hưởng nhiều vào kim loại có từ tính. mà sân năm 2013 dùng viền hoàn toàn bằng sắt nên khả năng ảnh hưởng bới sân thi đấu rất cao.

-Ưu điểm của Gyro:
+Quay với góc chính xác
+Ít phụ thuộc yếu tố bên ngoài
+Khá rẻ. Module GY521 tại một số nơi bán có 200k thôi.
+Thuật toán lập trình khá đơn giản.
+Chuẩn giao tiếp I2C​
-Nhược điểm:
+Dễ trôi điểm gốc. Nhưng chống trôi ổn thì chạy hết trận vẫn có thể có độ chính xác cao.
+Mình hay gặp trường hợp bị treo chip do việc đọc Gyro phải thực hiện trong ngắt timer. Mình lại dùng một số ngắt nữa nên hay xảy ra trường hợp xung đột ngắt. @@. Sau này mình hạn chế xử lý số liệu trong ngắt timer nên cũng khắc phục được lỗi nhưng vẫn không dám chắc là hết lỗi.​
-Hình ảnh thực tế:

[IMG]http://*************/attachments/6763/[/IMG]

​

-Hàm Init MPU6050: tài liệu hướng dẫn của PNlab đơn giản hơn. cái này mình tìm hiểu khi chưa có tài liệu tiếng việt mà dựa chủ yếu vào các code và tài liệu của các trang máy bay mô hình của nước ngoài nên lằng nhằng. @@.

void init_sensor_Gyro_MPU6050(void)

{

    delay_ms(10);

    i2c_writereg(MPU6050_ADD,0x6b,0x80);    //reset device, reset cam bien

    delay_ms(5);

    i2c_writereg(MPU6050_ADD,0x6b,0x00);    //cam bien hoat dong

    i2c_writereg(MPU6050_ADD,0x19,0x00);  //Sample Rate Divider = 0; cai nay minh khong hieu lam. @@

    i2c_writereg(MPU6050_ADD,0x1a,0x00);  //Configuration;

    i2c_writereg(MPU6050_ADD,0x1b,0x10);  //Gyroscope Configuration; 0-8-10-18 ; do phan giai Gyro la 1000do/s

    i2c_writereg(MPU6050_ADD,0x6a,0x00);  //User Control; Con MPU6050 co kha nang lam master

                                            //cua 1 so con có chuan truyen I2C khac, o day khong dung nen khong config

    i2c_writereg(MPU6050_ADD,0x37,0x02);

}

-Hàm lấy giá trị trong timer: do con này trả về giá trị là vận tốc góc nên ta tiến hành thực hiện phép tích phân để có giá trị góc do đó phải đưa việc tính toán vào ngắt timer.

// Timer2 overflow interrupt service routine
interrupt [TIM2_OVF] void timer2_ovf_isr(void)
{
// Reinitialize Timer2 value
          TCNT2=0x60;
          gyroADC[2]=-read_gyro_Z();
          goc=goc + ((float)gyroADC[2]+12.34)*0.0595/3.28;    //GY-521
          //                              [a]      [b]    [c]
          // 00    0    250    131
          // 08    1    500    65.5
          // 10    2    1000    32.8
          // 18    3    2000    16.4

}

Một số chú ý trong chương trình:
+ [a]: là hệ số chống trôi. mỗi cảm biến sẽ có một giá trị khác nhau. Để tìm hệ số này, các bạn cho hiển thị giá trị ADC đọc được của trục Z lên LCD khi cảm biến ĐỨNG YÊN hoàn toàn. [a]=-(giá trị đọc được). ADC của trục Z là số nguyên 16 bit có dấu. để có độ chính xác cao thì ta ép kiểu qua số thực để tính toán. Ví dụ như trên, khi hiển thị ADC Z thì giá trị sẽ dao động xung quanh giá trị -12. có thể là 11-14 nhưng giá trị -12 sẽ hiển thị nhiều nhất. Các bạn thay [a] vào và cho hiển thị giá trị góc khi CB ĐỨNG YÊN. các bạn sẽ thấy tuy không di chuyển cảm biến nhưng giá trị góc vẫn tăng hoặc giảm. giờ bước tinh chỉnh sẽ thuộc về các bạn để có giá trị 12,34 như của mình. mỗi cảm biến ở mỗi địa điểm khác nhau sẽ có 1 giá trị khác nhau nhé. Các bạn có thể thực hiện đọc Gyro lấy mẫu 100 lần khi CB đứng yên rồi chia ra cũng có giá trị này. Các trang nước ngoài thường làm thế. Song mình thấy mỗi lúc mỗi khác nên vẫn áp dụng phương pháp thủ công. @@

+[ b ]: thời gian lấy mẫu. Tuỳ vào giá trị timer của các bạn. Sau khi chống trôi xong thì việc tiếp theo của các bạn là đảm bảo quay đúng góc. Đưa cảm biến lên bề mặt phẳng hoàn toàn, quay 1 góc mà các bạn có thể xác định đc chính xác góc quay, đối chiếu với giá trị góc hiển thị trên LCD. Các bạn tiến hành tinh chỉnh b để có giá trị góc chính xác. Tăng hoặc giảm một lượng rất nhỏ.

+[c]: độ phân giải của cảm biến khi bạn config. [c] chính xác là số bít trên một đơn vị vận tốc. như trên config thì mình chọn khả năng làm việc của cảm biến là 1000 độ/s nên c=32.8. tuy nhiên để đạt độ chính xác cao hơn thì mình chọn c=3.28. là số bít trên 0,1 đơn vị vận tốc. Do đó thay vì ta quay 1 góc 90 độ thì giá trị góc trả về là 900.

[IMG]http://*************/attachments/6765/[/IMG]

​

Quan trọng nhất ở đây vẫn là việc chống trôi. các bạn chống trôi tốt có thể 3 phút thi đấu chỉ trôi 3 độ là cùng. sai số cho phép được.

Ở file đính kèm của mình có 2 file nén. file GY521mpu_6050.rar là mình lấy từ trang PNLab. file còn lại là mình dùng để làm thư viện chuẩn của mình. viết bằng Codevision

Cách kết nối và gá đặt cảm biến trên Robot:
-Như hình cảm biến thì các bạn chỉ cần quan tâm đến 4 chân đầu tiên của cảm biến là:GND, VCC, SDA, SCL.
+GND và VCC là 2 chân nguồn của cảm biến. áp là 5V. MPU6050 chỉ hoạt động điện áp 3v3. tuy nhiên trên module GY521 đã chuyển đổi điện áp nên mình không phải lo vấn đề này.
+SDA và SCL thì các bạn kết nối theo chuẩn I2C như các thiết bị I2C khác.
​-Gá đặt trên Robot. ở đây mình chỉ dùng trục Z để xác định góc quay do sân năm này ko có mặt phẳng nghiêng. với các sân có góc nghiêng thì các bạn phải tính thêm cả trục X và Y để có độ chính xác. Khi các bạn dùng trục Z thì để cảm biến nằm phẳng sao cho hướng trục Z trên cảm biến hướng lên trên. cố gắng càng hướng lên trên càng tốt để tránh sai số. nên gá ở khung đế robot để hạn chế xe dịch nhất. tránh xa các động cơ, cuộn dây vì các thiết bị này có từ trường và nhiễu lớn lên bus I2C của mình. CB thì không bị nhiễu nhưng Bus truyền rất dễ ảnh hưởng. có thể gá luôn lên trên mạch. Nhưng lúc này thì phải gá mạch cố định hoàn toàn.

Góp ý thêm cho các bạn. Nếu các bạn dùng cho máy bay mô hình thì có thể dùng module GY86. giá bây giờ khá rẻ. năm ngoái mình mua là 1200k. hiện giờ còn 700k ở PNLab và 500k ở thegioiic. Module này tích hợp MPU6050(Gyro, Acc, Nhiệt độ), HMC5883L(Mag hay la bàn số) và MS5611 (CB Áp suất). Khi dùng module này các bạn nên chú ý con la bàn. vì để giao tiếp với nó, các bạn phải giao tiếp với con MPU6050 như mình giải thích ở trên. con MPU6050 sẽ làm master và con HMC5883L làm slaver của con này. Cách giao tiếp các bạn có thể tham khảo code mình dưới đây.

cho em hỏi chút ạ, em thử dịch code của anh thì thấy code vision nó báo lỗi khai báo tùm lum hết lên, tại sao vậy ạ

[cuongpd]

cho em hỏi chút ạ, em thử dịch code của anh thì thấy code vision nó báo lỗi khai báo tùm lum hết lên, tại sao vậy ạ

Lỗi là đúng thôi, cái này chỉ giúp bạn hiểu để lập trình thôi. Vì vậy cần hiểu không phải copy code

[blogwhey1]

Lỗi là đúng thôi, cái này chỉ giúp bạn hiểu để lập trình thôi. Vì vậy cần hiểu không phải copy code

em code lại cho atmega 16 dựa trên code con at89s52 của banlinhkien.vn
theo code đó có hàm đọc và ghi

unsigned char Mpu6050_Read(unsigned char address)
   {
   bit AckTemp=1;
   unsigned char Data;
   I2C_Start();
   I2C_Write(mpu6050);
   AckTemp=I2C_CheckAck();
   I2C_Write(address);
   AckTemp=I2C_CheckAck();
   I2C_Start();
   I2C_Write(mpu6050+1);
   AckTemp=I2C_CheckAck();
   Data=I2C_Read();
   AckTemp=I2C_CheckAck();
   I2C_Stop();
   return Data;
   }
void Mpu6050_Write(unsigned char address,unsigned char Data)
   {
   bit AckTemp=1;
   I2C_Start();
   I2C_Write(mpu6050);
   AckTemp=I2C_CheckAck();
   I2C_Write(address);                  
   AckTemp=I2C_CheckAck();
   I2C_Write(Data);                
   AckTemp=I2C_CheckAck();
   I2C_Stop();
   }

em không hiểu khi chuyển qua avr thì thế nào do trên 89s52 có code được cho i2c vì định nhĩa được chân scl và sda, còn trong avr em không định nghĩa được 2 chân này để viết hàm tương tự , mà i2c trong avr cũng không có sẵn I2C_CheckAck(); nên khi build bị lỗi

[IMG]http://*************/attachments/55648/[/IMG]


em còn mắc ở phần này mong các anh chỉ hướng khắc phục ạ

[suckhoe280]

các bác cho em hỏi.
bây giở em muốn tính góc quay quanh trục Z thì làm sao. tức là khi để cảm biến nằm ngang, coi vị trí ban đầu là 0, bây giờ xoay cảm biến đi 1 góc nào đó (vẫn giữ cảm biến nằm ngang nhé) thì làm sao tính được góc quay đó vậy. cho e công thức tính toán với.
thanks