Khepera III 工具箱/程式設計提示
為 Khepera III 機器人編寫程式非常簡單。但是，良好的程式結構和第一次就做好事情會節省您的時間 - 大量的時間！因此，在本章中，我們將討論在實現此類程式時的一些設計選擇。
一個簡單的程式將具有以下結構
// Program initialization
commandline_init();
...
commandline_parse(argc, argv);
...
khepera3_init();
// Algorithm initialization
algorithm.configuration.wall_threshold = 1000;
...
khepera3_drive_start();
// Main loop
while (true) {
// Read sensors
khepera3_infrared_proximity();
...
// Calculate actuator response
speed_left = ...
speed_right = ...
// Termination condition
if (...) {
break;
}
// Set actuators
khepera3_drive_set_speed(speed_left, speed_right);
...
// Sleep for a while
usleep(algorithm.configuration.wait_us);
}
// Algorithm termination/cleanup if necessary
khepera3_drive_set_speed(0, 0)
大多數程式由一個主迴圈組成，該迴圈在初始化階段後執行。主迴圈實現了所謂的感知-動作迴圈 - 讀取感測器值、推匯出要採取的動作並相應地設定執行器（電機）的迴圈。主迴圈有時還包含一個終止條件，該條件在達到目標後立即退出程式。
如果您檢視現有程式的程式碼（例如 motion_followline），您會注意到上述結構在不同的函式中實現。通常，該結構會分解成以下函式
- help：列印幫助文字。
- algorithm_init：初始化演算法。
- algorithm_run：執行主迴圈。
- main：初始化程式並呼叫上述函式。
此外，還定義了一個結構來儲存演算法的配置和狀態（如果需要）。
// Algorithm configuration and state variables
struct sAlgorithm {
struct {
int wall_threshold;
...
int verbosity;
} configuration;
struct {
int remaining_targets;
...
} state;
};
// Declare an instance of that structure
struct sAlgorithm algorithm;
// Access to the variables
algorithm.configuration.wall_threshold = ...
algorithm.state.remaining_targets = ...
即使乍一看這似乎很複雜，使用這種巢狀結構也有兩個優點。首先，所有演算法變數都在一個地方宣告。如果您想在其他地方重用該演算法，您一目瞭然地看到需要哪些變數。其次，將配置變數和狀態變數分類將幫助您以乾淨的方式實現演算法。配置變數只會在初始化階段寫入，並且只應該在主迴圈中讀取。然而，狀態變數在主迴圈中讀寫。
通常，程式不止一個狀態。例如，一個參與搜尋任務的機器人可能會有一個探索狀態，在該狀態下它會隨機四處走動，以及一個梯度跟蹤狀態，在該狀態下它會嘗試向目標移動。
實現這一點的最簡單方法是使用不同的主迴圈，即每個狀態一個主迴圈。程式碼骨架如下所示（請注意，由 k3-create-program 指令碼 建立的模板程式使用的是這個骨架）。
// Algorithm variables
struct sAlgorithm {
...
struct {
void (*hook)(); // Pointer to the current state function
...
} state;
};
// Forward declaration of states
void state_exploration();
void state_gradient_follow();
void state_success();
// Algorithm initialization
void algorithm_init() {
...
// Set the initial state
algorithm.state.hook = state_exploration;
}
// The algorithm just switches from state to state by calling the current state function
void algorithm_run() {
while (1) {
algorithm.state.hook();
}
}
void state_exploration() {
// State initialization
...
// Main loop
while (1) {
// Read sensors
...
// State change condition
if (...) {
algorithm.state.hook = state_gradient_follow; // Switch to the gradient follow state
return;
}
// Set actuators
...
// Sleep for a while
usleep(algorithm.configuration.wait_us);
}
}
void state_gradient_follow() {
// State initialization
...
// Main loop
while (1) {
// Read sensors
...
// State change condition
if (...) {
algorithm.state.hook = state_exploration; // Switch to the exploration state
return;
}
// Termination condition
if (...) {
algorithm.state.hook = state_success; // Switch to the success state, which terminates the program
return;
}
// Set actuators
...
// Sleep for a while
usleep(algorithm.configuration.wait_us);
}
}
void state_success() {
// We are done
exit(0);
}
不要害怕函式指標（algorithm.state.hook）！它的宣告可能看起來很複雜，但它非常易於使用，如您在上面的狀態函式中看到的。新增、移動或刪除狀態變得極其容易，因為您可以將狀態視為一個連續的程式碼塊。如果機器人在一個特定狀態下沒有按預期工作，您可以修改其程式碼，而不會影響其他狀態（如果您將所有內容都放在一個帶有if串聯的主迴圈中，通常會發生這種情況）。
此外，如果您先在紙上繪製您的演算法作為狀態機，您就可以立即按照您在紙上的圖表實現程式碼。每個狀態將對應於一個函式，每個箭頭將對應於一個狀態變化條件。
從您的 控制理論 課程中，您可能知道感知-動作迴圈必須以一定的最小頻率執行，以滿足穩定性標準。另一方面，以更高的速度執行該迴圈也會消耗更多能量。
上面程式碼示例中顯示的感知-動作迴圈的速度由三個因素決定
- 通訊開銷，即讀取感測器和設定執行器所需的時間。
- 處理開銷，即從當前感知推匯出動作所需的時間。
- 等待時間（usleep(algorithm.configuration.wait_us)），可以隨意調整。
通訊開銷只能在一定程度上最佳化。您需要在機器人和 Korebot 板（處理器板）之間傳輸的位元組數有一個硬性限制，才能獲得一定的資訊，而通訊通道（I2C 匯流排）的固定速度約為 10 KB/s。因此，呼叫khepera3_infrared_proximity 函式來獲取紅外接近感測器值將在大約 3.1 毫秒後返回（如果 I2C 匯流排空閒），因為它必須從微控制器傳輸 31 個位元組到微控制器。每個函式傳輸的位元組數可以在 khepera3 模組 的 .h 檔案中找到。
類似地，為了在給定感測器讀數和當前狀態的情況下推匯出動作，需要進行一定數量的操作。與通訊開銷相比，基本數學運算速度很快，但大量使用log、exp 或三角函式會減慢主迴圈速度。此外，所有必須寫入或從磁碟讀取的內容也會涉及大量的開銷。最後但並非最不重要的是，printf會嚴重減慢您的程式速度，具體取決於輸出的重定向位置。
雖然可以準確計算通訊開銷，但處理開銷需要在程式執行的環境中進行測量（例如，將 stdout 重定向到將在真實實驗中重定向到的位置）。然後可以調整等待時間以實現所需的感知-動作迴圈頻率。
是否使用執行緒是計算機科學中的一場重大爭論。Khepera III 工具箱不會強迫您使用或不使用執行緒，但它是在大多數程式不會使用多執行緒的想法下編寫的。
如果您使用的是上面提供的程式碼骨架，那麼您自然不會使用執行緒（或者如果您想的話，可以使用一個執行緒）。考慮到沒有太多可並行化的地方，這是有道理的：機器人一次只能處於一種狀態，並且必須以精確的順序執行感知-動作迴圈的內部。
在編寫等待某些資料到達開啟的檔案控制代碼的程式時，select 系統呼叫 將很有用。此係統呼叫獲取檔案控制代碼列表，並在其中一個檔案控制代碼上可用資料或超時發生時返回。select 系統呼叫是解決大多數問題的方案，在這些問題中，程式設計師認為他們需要執行緒。
作為示例，請檢視 motion_arrowkeys 程式，該程式（以非阻塞方式）等待標準輸入上的使用者輸入，並同時執行主迴圈。
如果您出於某種原因需要使用多執行緒，則在訪問機器人感測器和執行器時需要謹慎。基本上，您不允許以併發方式訪問khepera3 模組或i2cal 模組，因為這些模組使用靜態分配的變數，因此不是執行緒安全的。對此有兩個解決方案
- 以這樣一種方式設計您的程式，即只有主執行緒（或另一個專用執行緒）訪問這些模組。在某些情況下，這很簡單，但在其他情況下可能會變得非常複雜。
- 同步對khepera3 模組的所有呼叫以及所有i2cal 事務。它們需要使用相同的互斥鎖進行同步，因為khepera3 模組使用i2cal 模組與感測器通訊，即大多數khepera3 函式是 I2C 總線上的事務。
如果您想知道：當前的實現有意不是執行緒安全的，因為這會使函式呼叫對單執行緒應用程式來說更加複雜和容易出錯。但是，將來可能會新增一個執行緒安全實現（例如，作為單獨的模組）。
就像我們將演算法的實現分解為每個實現一個狀態的函式一樣，我們也可以將它們拆分為不同的程式，每個程式實現一個狀態子集，並按正確的順序執行它們。這行得通，實際上在許多情況下都是一種非常好的技術！
假設你想要測試不同的演算法來遍歷一個迷宮。迷宮入口在你的競技場的左側，出口在右側。由於你想要多次執行實驗，所以你想要你的機器人一旦到達出口就自動回到入口，例如透過沿著地面的一條線。當然，你可以為所有演算法新增一個新的狀態（state_go_back_to_entrance），並在到達出口時切換到該狀態。但是，你也可以將其實現為一個單獨的程式（maze_go_back_to_entrance），在你的任何迷宮遍歷程式之後立即呼叫它。
啟動和退出程式會產生一些處理開銷，但從作業系統的角度來看，這種開銷很小。不過，你應該小心你的初始化階段。例如，如果你必須在程式啟動時載入大檔案（例如地圖等），這可能會產生一些明顯的延遲。此外，如果你透過 WLAN 連線遠端啟動程式，需要考慮一個小的傳輸延遲（~ 10 毫秒）。
關於何時拆分以及在同一程式中實現多少內容，並沒有明確的答案。但是，這裡有一些想法
- 如果這有助於你測試和除錯演算法的各個部分，請進行拆分。
- 如果這避免了在多個程式中包含完全相同的程式碼片段（如上面的迷宮遍歷示例），請進行拆分。
- 如果你認為可以將至少一部分程式碼重用於其他地方，請進行拆分。
- 如果兩個部分之間沒有任何共同之處，請進行拆分。
- 如果轉換很複雜（例如，多個目標狀態可能），請不要進行拆分。
- 如果精確的時間或低延遲是一個問題，請不要進行拆分。
- 如果兩個部分共享狀態變數或大量資料，請不要進行拆分。
此外，通常建議將演算法實現為僅執行一次，然後退出。如果你想在多次執行中測試你的演算法，只需多次啟動該程式（例如使用指令碼）並將輸出重定向到每次執行的不同檔案。這種方法更加靈活和健壯。
measure_real_speed 示例使用指令碼多次運行同一個程式，但使用不同的引數。