使用NXC撰寫NXT程式---第八章 再探循跡車

使用NXC撰寫NXT程式---第八章 再探循跡車


我們在第六章有寫過循跡車的程式如下:

//簡易循跡車
#define _LIGHT 45
#define _POWER 50
#define _TURN_TIME 100
task main()
{
  SetSensorLight(IN_3); //或用 S3
  while (true)
  {
    if (SENSOR_3<=_LIGHT) //左轉修正
    {
      NumOut(5,20,SENSOR_3,true);
      Off(OUT_C);
      OnFwd(OUT_B, _POWER); 
      Wait(_TURN_TIME);
    }
    else //右轉修正
    {
      NumOut(5,20,SENSOR_3,true);
      Off(OUT_B);
      OnFwd(OUT_C, _POWER); 
      Wait(_TURN_TIME);
    }
  } //end while
}

這個程式差強人意，走起路來歪七扭八怪彆扭的，我們接下來要使用PID控制的方式來讓循跡車走的更優雅。

關於PID控制，請先閱讀這篇 J. Sluka 所寫的文章，底下的作法仍根據這篇說明而來。

我們假設光感在全白的底色上所讀到的數據是50，而在黑色軌跡上的讀數是40，
上述程式的做法，是以這兩者的平均值45為判斷標準，
若機器人是沿黑線的左邊行進，小於等於45，表示偏向黑色軌跡，機器人需往左修正;
反之則以反方向修正。

不是左轉，就是右轉，以45為判斷的標準，典型的二元論。

難道沒有模糊地帶?

要讓機器人更聰明，必需讓它了解不僅只有這兩種情況。

假設我們的光感在不同的狀態下所讀到的數據如下:






至少現在我們可以有三種選擇:
1.小於45且大於等於40:向左修正
2.大於45且小於等於50:向右修正
3.等於45:直線前進

機器人現在多了一項反應了。

仔細一想，我們可以加入更多的情況讓機器人辨示，以做出更多不同的反應。




如上圖，我們現在可以有五種選擇:
1.在40到42之間:向左修正，且要修正大一點
2.在42到45之間:向左修正，但修正小一點即可
3.等於45:直線前進
4.在45到48之間:向右修正，但修正小一點即可
5.在48到50之間:向右修正，且要修正大一點

修正角度的大小，可以用馬達的輸出馬力來控制。

以此細分下去，我們可以讓光感讀到的數據，對映一個馬達輸出的馬力，
最簡單的算法，就是以線性等比例的方式來換算。

我們設定當光感讀數是45時，B、C馬達皆以50%的馬力輸出(即走直線)，
而在兩個極端點(即光感讀數為50及40時)，B、C兩個馬達的輸出為100%及0(或0及100%)，
我們可以畫出底下的函數圖形:





這條線的方程式為:
Power=10*(Light-45)+50
10即為斜率，在PID領域有個術語，稱為比例常數。

如果上面的函數圖形代表C馬達，
那我們可以讓B馬達的斜率為負，如下圖。



現在我們可以寫程式了。

//有比例控制的循跡車
//-----------------------------------------------
//請依您的情況調整Kp、offset與Tp值
//我的情況是Kp=2 、 offset=47 、 Tp=80 時走的最快
//而Kp=1 、 offset=47 、 Tp=50 時走的最平順
//-----------------------------------------------

task main()
{

int Kp = 10;//比例常數，調小此數值會走的比較平順
int offset = 45;//不修正時的光感值，依實際情況設定
int Tp = 50;//不修正時的馬力，愈大愈快，但要配合Kp

int Light;//光感讀數
int PowerB, PowerC;//B、C馬達的輸出馬力

SetSensorLight(IN_3); //或用 S3

while (true)
{


Light = SENSOR_3 ;//讀取光感數據
PowerC = (Kp*(Light-offset))+Tp;//代入方程式
PowerB = (-Kp*(Light-offset))+Tp;//另一個馬達的斜率是負的
TextOut(5,40,NumToStr(Light),true);
TextOut(5,20,NumToStr(PowerB));
TextOut(75,20,NumToStr(PowerC));

OnFwd(OUT_B,PowerB);
OnFwd(OUT_C,PowerC);

}

}

有可能在您調整參數的過程中，或是光感也有可能偵測到小於40或大於50的情況下，讓PowerC或PowerC大於100或小於0。雖然NXT對馬達的Power有自我保護，且NXC對小於0的Power值會自動逆轉，但程式師應該要對任何可能發生的意外負責，仔細考慮各種可能情況，否則，就會像福音戰士中的初號機一樣「暴走」，導致不可預期的失控。當然設計越複雜，就越難保證不失控。但從小事養成謹慎的態度是很好且必要的。我們的輸出入設計只限制在一個合理的範圍：在40<=Light<=50時，-100<=Power<=100。(Power為負值時逆轉)其他情況我們並沒有設計讓機器人如何反應，所以，我們要在程式碼中，好好的規範一下。底下是較有保障的程式。
註：
由於小弟偷懶，事隔1年半才又著手寫這篇文章，之前的程式不曉得誤了多少人，幸好不會讓NXT暴走或壞掉。可見寫網路文章也得戒慎恐懼啊！

//有比例控制且較穩定的循跡車
//-----------------------------------------------
//請依您的情況調整Kp、offset與Tp值
//我的情況是Kp=2 、 offset=47 、 Tp=80 時走的最快
//而Kp=1 、 offset=47 、 Tp=50 時走的最平順
//-----------------------------------------------

task main()
{

int LightMax = 50;//光感讀入的最大值
int LightMin = 40;//光感讀入的最小值
int PowerMax = 100;//Power的最大值
int PowerMin = -100;//Power的最小值
int Kp = 10;//比例常數，調小此數值會走的比較平順
int offset = (LightMax+LightMin)/2;//不修正時的光感值，依實際情況設定
int Tp = 50;//不修正時的馬力，愈大愈快，但要配合Kp

int Light;//光感讀數
int PowerB, PowerC;//B、C馬達的輸出馬力

SetSensorLight(IN_3); //或用 S3

while (true)
{

Light = SENSOR_3 ;//讀取光感數據
//限制輸入 Light 在比例控制的範圍
if (Light < LightMin)

Light=LightMin;

else if (Light > LightMax)

Light=LightMax;


PowerC = (Kp*(Light-offset))+Tp;//代入方程式
PowerB = (-Kp*(Light-offset))+Tp;//另一個馬達的斜率是負的
//限制輸出 Power 在比例控制的範圍
if (PowerC < PowerMin)

PowerC=PowerMin;

else if (PowerC > PowerMax)

PowerC=PowerMax;


if (PowerB < PowerMin)

PowerB=PowerMin;

else if (PowerB > PowerMax)

PowerB=PowerMax;


TextOut(5,40,NumToStr(Light),true);
TextOut(5,20,NumToStr(PowerB));
TextOut(75,20,NumToStr(PowerC));

OnFwd(OUT_B,PowerB);
OnFwd(OUT_C,PowerC);

}

}

(未完待續...)