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

[sangame]

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 độ.


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


Dưới đây là code của mình viết cho mục đích này:

void lay_lai_diem_goc(float goc_can_lay)
{                                 
    float sai_so_goc_2=0,kt_am_duong=0;//0: am; 1:duong;
 
    while (goc_toa_do<goc_can_lay) {goc_can_lay-=3600;}
    while (goc_toa_do>goc_can_lay) {goc_can_lay+=3600;}
 
    sai_so_goc_2=goc_can_lay-goc_toa_do;
 
    if (sai_so_goc_2>1800) {sai_so_goc_2=3600-sai_so_goc_2; kt_am_duong=0;goc_can_lay-=3600;}
    else kt_am_duong=1;
 
    if (kt_am_duong) {quay_trai(sai_so_goc_2,10);}
    else quay_phai(sai_so_goc_2,10);
}

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 di chuyển tịnh tiến:
Code di chuyển đa hướng:

update...

View more random threads:

• Robot Leo Tường
• Chim giả 2 đầu 31CS cực mềm đưa cặp đôi đến bến bờ đê mê nhất
• Khí nén sử dụng trong ROBOCON
• lập trình PID điều khiển mobile robot
• Xin hướng dẫn thiết kế cánh tay robot
• mobile robot dò đường
• Máy rửa board tự chế
• Tàu hai thân (Catamaran)
• Đề thi minirobocon MTA 2014
• Cheetah! ROBOT chạy nhanh hơn cả con người!

[congdong3]

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 đó:

int Trim( int x,int level )
{
  if( x > level ) return level;
  else if( x < -level ) return -level;
  else return x;
}

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:

int tinh_he_so(int goc_can_lay, float van_toc_goc)
{
    kp_c=van_toc_goc;             
    while ((-goc_toa_do)<goc_can_lay) {goc_can_lay-=3600;}
    while ((-goc_toa_do)>goc_can_lay) {goc_can_lay+=3600;}
    sai_so_goc_2= (0-goc_toa_do) - goc_can_lay;
    if (sai_so_goc_2>1800) {sai_so_goc_2=3600-sai_so_goc_2; goc_can_lay-=3600;}
    if (sai_so_goc_2<-1800) {sai_so_goc_2=3600+sai_so_goc_2; goc_can_lay+=3600;}
    if (sai_so_goc_2<0)kt_am_duong_goc_quay=0;
    else kt_am_duong_goc_quay=1;     
 
  if( absx(sai_so_goc_2) > 50 )
  {
      kp_c=van_toc_goc;
      ki_c = 0;
  }
  else
  {
      kp_c = 0;
      ki_c = 0.02;
  } 
   
  //...........................................................................
  //Tinh cac tham so P, I
  P_c = kp_c * (float)sai_so_goc_2;
  I_c += ki_c * (float)sai_so_goc_2;
 
  if( ki_c == 0 ) I_c = 0;
  else  I_c = Trim( I_c, I_C_MAX );
                                 
  output_c = (int)(P_c + I_c);
 
  output_c = Trim( output_c,55);
  return output_c;
}

/*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 đó. . đ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.

[IrwinGlass]

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 | Cộng đồng cơ điện tử Việt Nam | Mechatronics để hiểu rõ hơn đoạn code này.

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:

void di_chuyen( int van_toc_tinh_tien, float van_toc_goc, int goc_quay, int goc_tinh_tien )
{
 
  output_p = tinh_he_so( goc_quay, van_toc_goc );
 
  ctrl_speed_s1 = output_p + van_toc_tinh_tien * cos( ( goc_tinh_tien + goc_toa_do) - 1800 ) * PI / 1800.0 );
  ctrl_speed_s2 = output_p + van_toc_tinh_tien * cos( ( goc_tinh_tien + goc_toa_do) + 600 ) * PI / 1800.0 );
  ctrl_speed_s3 = output_p + van_toc_tinh_tien * cos( ( goc_tinh_tien + goc_toa_do) - 600) * PI / 1800.0 );
 
  ctrl_speed_s1 = Trim(ctrl_speed_s1,240);
  ctrl_speed_s2 = Trim(ctrl_speed_s2,240);
  ctrl_speed_s3 = Trim(ctrl_speed_s3,240);
 
  chay(ctrl_speed_s1,ctrl_speed_s2,ctrl_speed_s3);
}

ở 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.

[hangthanhly436]

Từ thời anh còn chơi robocon thì chưa biết đến mấy món này, cảm ơn em đã chia sẻ, tuy nhiên anh thấy thiếu cái gì đó. Không biết bài chia sẻ và hướng dẫn của em đã có nói đến phần cứng chưa?

[luuvanthuc]

Từ thời anh còn chơi robocon thì chưa biết đến mấy món này, cảm ơn em đã chia sẻ, tuy nhiên anh thấy thiếu cái gì đó. Không biết bài chia sẻ và hướng dẫn của em đã có nói đến phần cứng chưa?

Phần cứng mà anh nói đến ở đây có phải là mạch không ạ. phần cơ khí thì em đã giới thiệu ở bài Chia sẻ - Ứng dụng Gyro vào Robot chạy đa hướng trong Robocon | Cộng đồng cơ điện tử Việt Nam | Mechatronics . còn mạch thì em không muốn mọi người phụ thuộc vào phần cứng của em nên em không đưa lên. thật ra cũng đơn giản. mỗi động cơ được điều khiển theo thuật toán PID bởi một chip slaver giao tiếp theo chuẩn SPI. em xài 3 bánh nên có 3 slaver như vậy. hàm chay(td_dc1,td_dc2,td_dc3) sẽ thực hiện truyền tốc độ xuống các slaver.
Món này em cũng mới dùng ở vòng toàn quốc vừa rồi thôi ạ.

[pitias01]

Chuẩn rồi đây em, nhưng anh nghĩ nếu có thể nên chia sẻ, có cái gốc trước thì việc tham khảo và thực hành sẽ dễ hơn. Ngoài ra ứng dụng của nó rộng rãi chứ không hẳn gói gọn trong robocon đâu em ạ.

[thanhtrung]

Chuẩn rồi đây em, nhưng anh nghĩ nếu có thể nên chia sẻ, có cái gốc trước thì việc tham khảo và thực hành sẽ dễ hơn. Ngoài ra ứng dụng của nó rộng rãi chứ không hẳn gói gọn trong robocon đâu em ạ.

Dạ. Trước mắt là Robocon đã. sau này mọi người sẽ phát triển thành các ứng dụng cụ thể hơn. . nhưng mà một nhược điểm của phương pháp này là chi phí khá lớn. @@

[damynghebaoan]

Quan trọng là hiệu quả mang lại em ạ. Bình thường bỏ ra 20k để thu về 30k, nhưng nếu phải bỏ ra 50k để thu về 100k anh sẽ chọn phương án 2.

[tuyentravels]

dạ. . hehe. với cách di chuyển này em đã giảm được 1 lượng thời gian rất lớn trong việc di chuyển Robot. Nếu lập trình tinh chỉnh tốt có thể còn tăng tốc thêm, thay thế hoàn toàn việc dò đường.

[kimnhung1108]

Em làm tốt lắm An à, trước mắt thì chưa nhiều người hỏi em kỹ về phần này nhưng anh hy vọng trong đợt Robocon tới tất cả các đội đều có thể dùng phương pháp di chuyển của em để thay thế dò line. Sang năm anh hy vọng có nhiều trận hay, và lạc hồng không phải là cái tên quá nổi trội nữa ).