Please enable Javascript And Refesh (F5)

Một số code liên quan đến di chuyển đa hướng - Viet Nhap. Com 
no comments

Một số code liên quan đến di chuyển đa hướng

Loading...

Sau bài viết giới thiệu về cách di chuyển đa hướng, mình muốn chia sẻ với các bạn có quan tâm đến Robocon code mình viết cho Auto 2013. Code mình viết chưa tối ưu nên mong muốn các bạn góp ý để hoàn thiện thêm. :)

Code tính toán góc: mục đích của đoạn code này là mình muốn tính toán chọn được chiều quay hợp lý cho Robot. Như Hình vẽ sau các bạn sẽ thấy rõ. giả sử như cảm biến đã quay lệch một góc 30 độ. giờ các bạn muốn quay sang góc 120 độ thì làm thế nào. vì góc 30 độ có thể là cảm biến đã quay 360 độ + 30 độ. vì góc ở đây là góc đại số tính theo Gyro nên có thể lớn hơn 360 độ hoặc nhỏ hơn 0 độ. mục đích của hàm này là dù bất kể Gyro đã quay bao nhiêu vòng thì vẫn có thể tìm được hướng ngắn nhất để quay đến đích. Ở đây quay hướng A (quay 90 độ) sẽ ngắn hơn hướng B (270 độ). Và một vấn đề nữa là làm sao Robot ko quay quá nhiều vòng rồi mới đến đích. tức là 90 + 360*x độ.
dr2.
Dưới đây là code của mình viết cho mục đích này:

Mã:

chú thích các biến:
goc_toa_do: là góc thực của cảm biến.
goc_can_lay: là góc cần đạt được sau khi quay.
sai_so_goc_2: là góc ngắn nhất sau khi tính toán để quay.
kt_am_duong: kiểm tra chiều quay của robot.
quay_phai(sai_so_goc_2,10); hàm quay phải một góc sai_so_goc_2 với tốc độ 10/250.
quay_trai(sai_so_goc_2,10); hàm quay trái một góc sai_so_goc_2 với tốc độ 10/250.
3600 là 360 độ. do mình tính toán với đơn vị là 0,1 độ nên giá trị 1 góc là 3600. mình tính như vậy để kết quả mịn hơn thôi. :)

Code tính toán việc quay Robot:
Hàm xén tín hiệu: Cái này mục đích giới hạn hệ số tốc độ động cơ không quá một giới hạn nào đó:

Mã:

Hàm tính hệ số vận tốc quay dựa vào góc cần quay so với góc hiện tại của Robot và vân tốc góc cần quay:

Mã:

/*Code có tham khảo ở nguồn khác*/

Ở đây đoạn đầu thì giống với hàm ở #1, đoạn sau là hàm tính toán dựa trên góc sai lệch tính toán. Mình chia ra 2 trường hợp: khi góc lớn hơn 5 độ và khi góc nhỏ hơn 5 độ. sở dĩ được chia như vậy vì cơ bản thì vận tốc quay được tính dựa trên góc sai lệch nhân với vận tốc góc. Như vậy nếu góc sai lệch quá nhỏ thì sẽ không làm quay được Robot, vì vậy phải tính theo phương pháp cộng dồn sai lệch (tích phân) tính theo hệ số ki cho trước. ở đây kp chính là vận tốc góc còn ki=0,02. Đoạn code trên khá ngắn và dễ hiểu nen có lẽ mình không cần phải giải thích thêm, nó gần giống với thuật toán PID vậy. :)
Lời khuyên: các bạn nên đọc hiểu rồi code lại theo cách riêng mỗi người, code theo mình có thể sẽ sai đó. :D. đoạn code này các bạn chỉ nên tham khảo vì lúc đầu hàm tính góc của mình bị ngược chiều nên việc tính toán sẽ ngược, lúc thi đấu mình nhát sửa lại nên để thế luôn. :)) . các bạn chú ý ở dòng sai_so_goc_2= (0-goc_toa_do) – goc_can_lay; nhé. Mình sẽ đưa toàn bộ code auto của mình lên sau. :)

Cuối cùng là Code di chuyển đa hướng: các bạn nên xem lại bài Chia sẻ – Ứng dụng Gyro vào Robot chạy đa hướng trong Robocon

theo bài ở trên thì:

V[A] = V[tt] * cos(a+Góc hiện tại của Robot-180) + Vquay * sai số góc
V[ B] = V[tt] * cos(a+Góc hiện tại của Robot+60) + Vquay * sai số góc
V[C] = V[tt] * cos(a+Góc hiện tại của Robot-60) + Vquay * sai số góc

Hàm trên được Code lại như sau:

Mã:

ở hàm này thì phần quay đã được xử lý ở hàm ở #2. phần còn lại nằm ở phần tịnh tiến. có 4 dữ liệu cần đưa vào hàm và ta đã xử lý được nó. :). các tính toán ta xử lý ở đơn vị độ nên cần chuyển sang radian như trên. 

Chúc các bạn thành công với cách di chuyển này. :)

 

BÌNH LUẬN

avatar