Tải bản đầy đủ - 0 (trang)
3 Phương pháp xác định góc độ

3 Phương pháp xác định góc độ

Tải bản đầy đủ - 0trang

Giá trị gia tốc góc x Thời gian lấy mẫu

Vận tốc góc

Giá trị gia tốc góc

Gia tốc góc



Hình 4.11: Xác định góc nghiêng và gia tốc riêng biệt theo gia tốc

Cách tính góc nghiêng từ gia tốc.

Góc nghiêng tính được từ giá trị gia tốc và góc nghiêng tính được từ giá trị vận tốc

góc thường có sự chênh lệch nhau. Hơn nữa vận tốc góc từ cảm biến là vận tốc tức thời,

tức khơng duy trì và bằng khơng khi cảm biến khơng di chuyển, còn gia tốc là gia tốc

tĩnh, duy trì giá trị khi cảm biến ở một vị trí nào đó.



Hình 4.12: Cách tính góc nghiêng từ gia tốc.

Áp dụng cơng thức:

Trong đó:

: Gia tốc trọng trường

: Gia tốc góc theo phương Y

Ax: Gia tốc góc theo phương X

: Là góc

32



Do đó, chúng ta cần xác định góc nghiêng từ cả giá trị gia tốc và vận tốc góc từ

cảm biến để thu được kết quả chính xác.



Phương trình tốn học kết hợp giá trị vận tốc góc và gia tốc góc

Vận tốc góc



Gia tốc góc



Hình 4.13: Mơ tả cách xác định góc nghiêng khi kết hợp gia tốc và vận tốc.

Chúng ta có thể kết hợp giá trị gia tốc và vận tốc góc để xác định góc nghiêng.

Cũng lưu ý rằng việc tìm góc nghiêng từ những dữ liệu thu được từ cảm biến MPU6050,

cụ thể ở đây là các giá trị gia tốc và vận tốc góc rất nhạy và chịu ảnh hưởng bởi nhiễu rất

lớn. Môi trường đặt cảm biến càng mất ổn định thì góc nghiêng thu được càng sai lệch

nhiều. Góc nghiêng thu được có sai số rất lớn khi chỉ đơn thuần tính góc nghiêng từ

những dữ liệu đọc được từ cảm biến. Do đó, cần áp dụng một giải thuật tối ưu để tính

tốn góc nghiêng được chính xác.

Cách tính góc theo phương đứng:

Y = ypr[1] * 180/M_PI;

if(Y<0)Y=Y *(-1);

Y=Y-offset_Y;

if(Y<0)Y=Y *(-1);

Cách tính góc theo phương ngang:

X = ypr[2] * 180/M_PI;

if(X<0)X= X*(-1);

X=X-offset_X

if(X<0)X = X*(-1)

33



Các giá trị đọc được từ cảm biến thường chứa nhiễu, tác động bởi nhiều nguồn

khác nhau, như nhiễu quá trình, nhiễu nhiệt, nhiều từ bộ biến đổi AC/DC. Giá trị đọc

được do đó dao động ngay cả khi giá trị thực là hằng số.

Để giảm được các sai số, và chống nhiễu bởi các tác động bên ngồi ta sử dụng

phép tính lấy giá trị trung bình của các giá trị mà cảm biến đọc được rồi truyền về vi điều

khiển

Theo yêu cầu đồ án thiết bị thước thủy có thể đo được góc bằng hoặc nhỏ hơn 180

độ. Do vậy thuật toán được thiết lập :

Thuật tốn tính góc theo phương ngang:

X = ypr[2] * 180/M_PI;

if(X<0)X= X*(-1);

Giá trị X được tính như sau: Ta lấy giá trị trung bình của các giá trị mà cảm biến

đọc được nhân cho 1800, tất cả chia cho PI (3,14…). Ta sẽ có giá trị của góc X. nếu cảm

biến nghiêng về âm ta lấy giá trị góc X vừa tìm được nhân với -1.

Do thiết bị được sử dụng đo nhiều vị trí nhiều điểm đặt khác nhau, nên cảm biến có

thể dễ bị nhiễu nên thiết bị thước thủy thể hiện kỹ thuật số được thiết kế nút nhấn dùng để

SET lại giá trị đưa giá trị về khơng trước khi đo để có thể giảm được độ sai số. Thuật toán

được thiết lập như sau:

X=X-offset_X

if(X<0)X = X*(-1)

Giá trị góc X đo các vị trí kế tiếp sẽ được tính. Lấy giá trị góc X của góc đo trước

trừ đi giá trị offset (X) ta sẽ có được giá trị góc X sẽ ít bị tác động bên ngoài. Nếu cảm

biến nghiêng về âm ta lấy giá trị góc X vừa tìm được nhân với -1.

Tương tự thuật toán theo phương ngang (X). Ta cũng thiết lập được thuật toán

phương đứng (Y).



34



4.3.2 Cách truyền dữ liệu của cảm biến MPU6050 bằng chuẩn giao tiếp I2C

I2C là một loại bus giao tiếp ngoại vi được phát triển bới hãng Philips, thường

được sử dụng để điều khiển nhiều thiết bị điều khiển khác được bố trí trong các khối

mạch tương đối xa nhau. I2C là giao tiếp nối tiếp thực hiện trên hai đường dây: Clock

(SCL) và Data (SDA) giữa IC chủ và IC trạm.



Master : IC chủ.

Slave : IC trạm

Hình 4.14: Kết nối giữa Master và các Slave.

IC chủ: Là IC khởi động quá trình truyền nhận, phát đi địa chỉ của thiết bị cần giao

tiếp và tạo xung giữ nhịp trên đường SCL.

IC trạm: Là IC có một địa chỉ cố định, được gọi bởi Master và phục vụ yêu cầu từ

Master.

SDA - Serial Data: Là đường dữ liệu nối tiếp, tất cả các thông tin về địa chỉ hay dữ

liệu đều được truyền trên đường này theo thứ tự từng bit một.

SCL - Serial Clock: Là đường giữ nhịp nối tiếp. I2C là chuẩn truyền thông nối tiếp

đồng bộ, cần có 1 đường tạo xung giữ nhịp cho quá trình truyền/nhận, cứ mỗi xung trên

đường giữ nhịp SCL, một bit dữ liệu trên đường SDA sẽ được lấy mẫu.



35



Hình 4.15: Trạng thái lấy mẫu một bit dữ liệu trên SDA

Dữ liệu nối tiếp trên đường SDA được lấy mẫu khi đường SCL ở mức cao trong

một chu kỳ giữ nhịp, vì thế đường SDA khơng được đổi trạng thái khi SCL ở mức cao

(trừ Start và Stop condition). Chân SDA có thể được đổi trạng thái khi SCL ở mức thấp.

Các tín hiệu Start và Stop: SDA = SCL = HIGH: Bus I2C ở trạng thái sẵn sàng cho

một giao tiếp. Điều kiện Start: SDA và SCL = HIGH. Điều kiện Stop: SDA ↑ và SCL =

HIGH. Cả hai điều kiện Star và Stop đều được tạo ra bởi Master. Sau tín hiệu Start, bus

I2C ở trạng thái “busy” thực hiện tác vụ truy xuất nối tiếp. Sau Stop bus I2C ở trạng thái

“free” cho lần kế tiếp.

Định dạng dữ liệu truyền:

Dữ liệu được truyền trên bus I2C theo từng bit tại mỗi cạnh lên của xung Clock

(SCL).



Hình 4.16: Tín hiệu xung SCL và SDA.



Sự thay đổi bit dữ liệu trên SDA xảy ra khi SCL đang ở mức thấp. Số lượng byte

có thể truyền trong một lần tùy ý, tối đa là 128 bytes. Bit MSB sẽ được truyền trước.

Sau 8 xung clock 8 bit dữ liệu đã được truyền đi, thiết bị nhận sẽ SDA xuống

mức thấp tương ứng với một bit ACK sẽ tiếp tục thực hiện quá trình truyền hoặc kết

thúc.

Định dạng địa chỉ thiết bị: Mỗi thiết bị ngoại vi tham gia vào bus I2C đều có địa chỉ

duy nhất, có độ dài là 7 bit như vậy trên một bus I2C ta có thể phân biệt tối đa 128 thiết

36



bị. Byte đầu tiên được gửi từ master sẽ bao gồm 7 bit chỉ và bit thứ 8 gọi là bit R/W. Bit

R/W = 0 : Ghi dữ liệu từ Master vào Slave. Bit R/W = 1: Đọc dữ liệu từ Slave .

Sau đây là khung truyền và đọc dữ liệu

Đọc byte đơn



Hình 4.17: Đọc byte đơn.

Đọc nhiều byte



Hình 4.18: Đọc nhiều byte.

Truyền byte đơn



Hình 4.19: Truyền byte đơn.



Truyền nhiều byte



Hình 4.20: Truyền nhiều byte.

Trong đó:

S là start SDA xuống thấp trong khi SCL đang ở mức cao.

AD là địa chỉ của slave.

W là bit write (0).

R là bit read (1).

37



ACk bit xác nhận đã nhận dữ liệu.

RA địa chỉ chứa dữ liệu của slave.

DATA dữ liệu được truyền hoặc nhận.

4.4 Lưu đồ giải thuật điều khiển thiết bị thước thủy thể hiện dạng kỹ thuật số



Bắt đầu



Đọc dữ liệu (θ0)từ cảm biến MPU6050

Đọc số liệu (R0) từ biến trở



Tính góc θ1, θ2 từ giá trị của θ0



Tính góc R từ giá trị của R0

Trong đó:

Hiển thị giá trị góc đo θ1

R0: Giá trị đo được của biến trở

Hiển thị giá trị góc đo θ2

R: Giá trị góc đo tính tốn từ R0

Hiển thị giá trị góc đo R

θ0: Góc đo được của cảm biến MPU6050.

θ1: Giá trị góc nghiêng theo phương X do cảm biến MPU6050 tính tốn

θ2: Giá trị góc nghiêng theo phương Y do cảm biến MPU6050 tính tốn

Sai

Kết thúc ?



Đúng

Kết thúc

38



Hình 4.21: Lưu đồ giải thuật điều khiển thiết bị thước thủy thể hiện dạng kỹ thuật số

Lưu đồ giải thuật điều khiển thiết bị thước thủy thể hiện dạng kỹ thuật số thể hiện

trên (Hình 4.21). Bắt đầu giải thuật là đọc các giá trị θ0 từ cảm biến cân bằng GY-521

MPU6050, giá trị R0 từ biến trở. Bước tiếp theo là tính tốn ra các giá trị góc nghiên theo

phương X (θ1), giá trị góc nghiên theo phương Y (θ2) từ giá trị đọc θ0. Tính tốn giá trị góc

R từ giá trị đọc từ biến trở (R0). Sau khi đã tính tốn được các giá trị θ1 ,θ2,R. Sau khi tính

tốn đưa ra giá trị, hiển thị các giá trị ra thiết bị hiển thị. Sau khi giải thuật đã xong, nếu

không muốn kết thúc thì các bước sẽ bắt đầu lại từ bước đọc giá trị R 0 và θ0. Nếu không

muốn tiếp tục thì kết thúc.

4.4.1 Lưu đồ giải thuật đọc cảm biến GY-521 MPU6050

Lưu đồ giải thuật đọc dữ liệu cảm biến được thể hiện trên (Hình 4.22). Khi ta bắt

đầu cho thiết bị hoạt động, cảm biến góc gia tốc GY-521 MPU6050 sẽ tự động đọc dữ

liệu ACCEL của góc X, Y và Z. Sau đó sẽ xử lý số liệu về vận tốc góc GYRO.

Sau khi xử lý dữ liệu về vận tốc góc. Cảm biến sẽ sử dụng thuật tốn để đưa dữ

liệu về dạng góc thực tế.

Thuật toán:

G_x = arctan(ACCEL_X, ACCEL_Z)*180 / PI

G_y = arctan(ACCEL_Y, ACCEL_Z)*180 / PI

Theo yêu cầu của đề tài thiết bị thước thủy có thể đo góc bằng và nhỏ hơn 180 0 nên

ta có thể thiết lập được thuật tốn để đưa dữ liệu về góc thực tế

Góc X (phương ngang ) được tính như sau : Ta lấy arctan của dữ liệu ACCEL_X

và ACCEL_Z nhân cho 1800 sau đó chia tất cả cho PI.

Tương tự như tính góc X ta cũng có thể tính được góc Y.

Sau khi đọc và xử lý số liệu đưa dữ liệu về góc thực cảm biến góc gia tốc GY-521

MPU6050 sẽ truyền dữ liệu đã được xử lý về cho vi điều khiển.

39



Tài liệu bạn tìm kiếm đã sẵn sàng tải về

3 Phương pháp xác định góc độ

Tải bản đầy đủ ngay(0 tr)

×