Để có thể điều khiển đƣợc xe tự hành, sử dụng một chƣơng trình đƣợc hiện thực trên máy tính. Chƣơng trình điều khiển này sẽ đọc tập tin chứa đƣờng đi ngắn nhất và truyền dữ liệu đƣờng đi tới cho xe tự hành thông qua bluetooth. Sử dụng ngôn ngữ C# để viết chƣơng trình điều khiển thông qua Microsoft Robotics Developer Studio (MRDS).
Chƣơng trình này sẽ nhận input đầu vào chính là output đầu ra của chƣơng trình tìm đƣờng đi ngắn nhất đã đƣợc trình bày trong phần 3.3.2.1. Tập tin này cho xe tự hành biết một chuỗi các tọa độ cần di chuyển. Việc di chuyển từ tọa độ A(xA,yA) tới B(xB,yB) đƣợc thực hiện bằng cách xe tự hành
đang từ A, sẽ quay một góc nào đó, về phái trái hay phải và di chuyển một đoạn bằng khoảng cách từ A tới B.
Nhƣ vậy, có ba công việc chính của chƣơng trình điều khiển là:
- Tính toán độ chuyển hƣớng, khoảng cách di chuyển cho xe tự hành; - Giao tiếp với xe tự hành thông qua bluetooth;
- Điều khiển xe tự hành xoay và chạy chính xác với một góc và một khoảng cách biết trƣớc.
Tính toán độ chuyển hƣớng và khoảng cách di chuyển cho xe tự hành đƣợc tính toán bằng các công thức hình học cơ bản. Chƣơng trình giao tiếp với xe tự hành thông qua bluetooth đƣợc hỗ trợ bởi MRDS, chi tiết nhƣ thế nào để khởi động bluetooth trên máy tính, trên xe tự hành và các thao tác để chúng có thể giao tiếp với nhau đƣợc trình bày trong phụ lục. Điều khiển xe tự hành xoay và chạy chính xác với một góc và một khoảng cách biết trƣớc nhƣ code trình bày trong Bảng 5.1.
MRDS hỗ trợ một số hàm nhƣ RoteDegrees hoặc DriveDisnce hỗ trợ xe tự hành chuyển hƣớng một góc hoặc chạy với một khoảng cách đƣa vào. Tuy nhiên, những hàm này đƣợc hỗ trợ tổng quát cho nhiều xe tự hành có thể với MRDS. Khi sử dụng cụ thể với Lego Mindstorm NXT 2.0, có thể sử dụng những hàm này bằng cách gán cụ thể bán kính bánh xe của NXT vào. Những hàm này đƣợc MRDS hiện thực bằng cách định thời, cho bánh xe chuyển hƣớng và đếm số vòng quay bánh xe, sau một khoảng thời gian, đúng độ cần quay hoặc khoảng cách cần di chuyển, xe tự hành sẽ đƣợc cho dừng lại. Tuy nhiên, nếu chạy với mô phỏng của MRDS thì những hàm này cho kết quả rất tốt, nhƣng với môi trƣờng thực tế thì việc định thời từ MRDS truyền đến xe tự hành NXT vẫn cho kết quả sai lệch trong một số trƣờng hợp kiểm thử. Vì vậy, để đạt đƣợc độ chính xác cao trong việc queo một góc và di chuyển một đoạn, chƣơng trình không sử dụng hàm bổ trợ của MRDS nhƣ trình bày ở trên, mà việc định thời, đo thời gian chuyển hƣớng hay chạy đƣợc hiện thực lại (nhƣ code trong Bảng 5.1).
Để xe tự hành quay một góc α độ, đầu tiên, hai bánh xe trái và phải của xe tự hành phải quay ngƣợc chiều nhau cùng một vận tốc để xoay; sau một khoảng thời gian θ, xe tự hành đã quay đúng α độ và hai bánh xe của xe tự hành dừng lại. Nhƣ thế nào để xác định θ dựa vào một bảng đối chiếu giữa thời gian và số độ của góc quay thực tế của robot NXT.
Để xe tự hành đi/chạy một đoạn d, đầu tiên, hai bánh xe trái và phải của xe tự hành phải quay cùng chiều nhau và cùng một vận tốc để xe tự hành đi thẳng; sau một khoảng thời gian θ’, xe tự hành đã đi đúng khoảng d, hai bánh
xe của xe tự hành dừng lại. Nhƣ thế nào để xác định θ’ dựa vào một bảng đối chiếu giữa thời gian và khoảng cách di chuyển thực tế của robot NXT.
Bảng 5.1. Chương trình điều khiển Lego Mindstorm NXT bằng C# [3]
1 yield return Arbiter.Receive( 2 false, 3 TimeoutPort(10000), 4 delegate(DateTime timeout) { } 5 ); 6 rotePower = 0.25; 7 runPower = 0.4;
8 for (int times = 0; times < repeatCount; times++) 9 {
10 roteTime = (int)rote_list[times]; 11 driveTime = (int)drive_list[times]; 12 roteDirect = direct_list[times]; 13
14 if (roteDirect == 1) //turn left
15 {
16 _nxtDrivePort.DriveDisnce(rotePower, - rotePower, 0); 17 yield return Arbiter.Receive(
18 false,
19 TimeoutPort(roteTime),
20 delegate(DateTime timeout) { });
21 }else //turn right
22 {
23 _nxtDrivePort.DriveDisnce(-rotePower, rotePower, 0); 24 yield return Arbiter.Receive(
25 false,
26 TimeoutPort(roteTime),
27 delegate(DateTime timeout) { }); 28 }
29
30 _nxtDrivePort.DriveDisnce(0, 0, 0); // Stop the motors 31 yield return Arbiter.Receive(
32 false,
33 TimeoutPort(settlingTime), 34 delegate(DateTime timeout) { }); 35
36 _nxtDrivePort.DriveDisnce(runPower, runPower, 0); 37 yield return Arbiter.Receive(
38 false,
39 TimeoutPort(driveTime),
40 delegate(DateTime timeout) { }); 41
42 _nxtDrivePort.DriveDisnce(0, 0, 0); 43 yield return Arbiter.Receive(
44 false,
45 TimeoutPort(settlingTime), 46 delegate(DateTime timeout) { }); 47 }
Từ dòng 1 đến 5, xe tự hành dừng một khoảng thời gian để khởi tạo; sau đó vào vòng lặp, thực hiện một loạt các thao tác xoay và di chuyển tới. Giả sử xe tự hành cần đi qua một chuỗi các đỉnh A1, A2, A3 … An để đến đích. repeatCount trong vòng lặp sẽ bằng n. Trong mỗi vòng lặp, xe tự hành thực hiện việc di chuyển từ Ak đến Ak+1. Để làm đƣợc việc này, xe tự hành đầu tiên cần xoay một góc α sao cho đầu xe tự hành hƣớng đến đúng Ak+1, sau đó xe tự
hành di chuyển một khoảng dk để đến Ak+1. αk và dk đƣợc chuyển đổi sang khoảng thời gian tƣơng ứng. Các khoảng thời gian tƣơng ứng cho việc xoay và đi thẳng đƣợc lƣu trong rote_list và drive_list, hƣớng xoay đƣợc lƣu trong
direct_list. Từ dòng 14 đến 28, việc xoay góc phải hoặc trái đƣợc thực hiện. Dòng 16 và 23 khởi tạo hai bánh xe quay ngƣợc nhau (dòng 16 xoay trái và dòng 23 xoayphải). Việc khởi tạo này có thể dùng DriveDisnce với tham số
cuối là khoảng cách di chuyển phải đƣợc truyền là zero (tham số cuối quyết định khoảng cách di chuyển của xe tự hành, nhƣng nhƣ đã trình bày ở trên, việc di chuyển này sẽ không chính xác nên chỉ cần truyền zero). Dòng 17 đến 20 định thời cho xe tự hành. Dòng 16 làm xe tự hành xoay, sau khoảng thời gian roteTime, xe tự hành sẽ dừng lại. Khi dừng lại sau khoảng roteTime, xe tự hành quay đúng góc mong muốn. Dòng 30 đến 34, xe tự hành dừng mô tơ lại một khoảng thời gian settlingTime để đảm bảo chính xác cho hoạt động tiếp theo. Dòng 36 đến 40 điều khiển xe tự hành đi thẳng một đoạn. Trong dòng 36, hai bánh xe đƣợc khởi tạo quay cùng hƣớng, cùng vận tốc với nhau để xe tự hành có thể đi thẳng, thay vì trái dấu nhau nhƣ trong 16 hoặc 23 để xe tự hành xoay. Tƣơng tự, dòng 37 đến 40 cũng dùng để định thời cho xe tự hành, sau một khoảng thời gian driveTime xe tự hành sẽ dừng lại đúng vị trí Ak+1. Sau khi đi thẳng một đoạn, xe tự hành cũng đƣợc dừng một khoảng để hoạt động tiếp theo chính xác hơn. Vòng lặp cứ thế tiếp tục cho đến khi xe tự hành di chuyển đến đích cần đến.
Hình 5.5 - Kết quả kiểm tra thực tế