篩選簡化路徑的Line Follower

續前一篇[具路徑規劃的Line Follower]，當電腦鼠(nxt car)從起點走到終點後，根據前述的程式，我們是將所經過的路徑之x-y座標，存成檔案紀錄在nxt檔案夾中，可供我們事後觀測座標路徑正確與否。

其次，我們打算根據所走的x-y所有點，計算較短路徑。


(圖1 電腦鼠軌道)

程式的規劃，電腦鼠在路徑行走時，採右手法則(即遇到叉路時，以右轉優先，其次為直行，最後為左轉)。
從圖1，可以知道:
當電腦鼠走到(-1,1)時，以右手法則轉向右邊，途經(-1,2)、(-1,3)、(-2,3)、(-2,2)、(-2,1)，最後又會到(-1,1)的點，之後右轉至(-1,0)，再轉至終點(-2,0)。
這過程中，電腦鼠途經(-1,2)、(-1,3)、(-2,3)、(-2,2)、(-2,1)這些座標的點，對我們直觀的判斷，這些中途繞道的點，應該是多餘的。所以之後的程式，應該根據所有走過的座標，去篩選哪些是不必要再走的。

本篇文章，先設計一測試程式，將電腦鼠走過的座標，存成陣列，並逐步篩選以簡化路徑。

[程式]

(圖2 簡化路徑之程式)

圖2程式:
x[ ],y[ ]為原始數據，而x_fix[ ], y_fix[ ]為簡化後記錄的座標數據
程式從第一組元素(element)開始往後比對，i為參考組，j為比對組。
後面各組(j)若有與參考組(i)的座標雷同，則記錄該點至x_fix及y_fix。
(基本上，當電腦鼠會走回原來的地方，代表剛剛所經過的路徑必不是終點存在的區域，既然回到原來的地方，所以剛剛繞過的地方可捨去不用)


(圖3 電腦鼠走過的座標示意圖)

從圖3，籃框處中間的其他各點，為非必要的途徑，所以可捨去。
所以經過篩選後，最後的座標為:
(0,1)-->(-1,1)-->(-1,0)-->(-2,0)

具路徑紀錄之Line Follower

前些日子，採購了一顆具備辨色能力之hitechnic color sensor，並應用在Line Follower實務，在等距之處貼上色貼，刻劃橫坐標x以及縱座標y。


(圖1 硬體地圖配置)

一、硬體介紹
1.圖1每一個色貼間隔，盡可能讓bot有足夠空間可以迴轉。

2.起點設置在照片中車子停止的位置，硬體裝置使用三個感應器(light sensor x2、color sensor x1)。color sensor負責偵測前端是否為[色貼]標籤，做為後續判斷是否前進或左右轉彎的依據。

3.light sensor分置兩側，左邊light sensor(緊鄰color sensor)，負責做沿線前進的功能；右邊的light sensor，負責判斷是否做轉彎的動作…

二、軟體程式

#define LMotor OUT_A

#define RMotor OUT_C

#define Motors OUT_AC



#define LightSensor1Port IN_1

#define LightSensor2Port IN_2

#define ColorSensorPort IN_4

#define factor 2



#define black_tap 0

#define blue_tap1 3

#define blue_tap2 4

#define red_tap   9





int colornum;               //彩色感應器讀值

int black,white,light1_value,light2_value; //光感器讀值

int Lpower=0,Rpower=0;

long tick=0;



//紀錄車子移動方位



int x=0,y=0;

int D=1;    //初始放在y軸上



byte fileHandle;

short bytesWritten;

string str_log;

string str_x;

string str_y;



void InitialLightSensor()

{

    while(!ButtonPressed(BTNCENTER,false))

    {

        TextOut(0,LCD_LINE3,"Light at black..");

        black=Sensor(LightSensor1Port);

    }

    ClearScreen();

    Wait(1000);

    while(!ButtonPressed(BTNCENTER,false))

    {

         TextOut(0,LCD_LINE3,"Light at white..");

         white=Sensor(LightSensor1Port);

    }

    ClearScreen();

    tick=CurrentTick();

    while(CurrentTick()<(tick+3000))

    {

        PlayTone(880,50);

        Wait(950);

    }

    Wait(1000);

}



void BotWalking()

{

     light1_value=Sensor(LightSensor1Port);

     Rpower=(light1_value-black)*factor;

     Lpower=(white-light1_value)*factor;

     OnFwd(LMotor,Lpower);

     OnFwd(RMotor,Rpower);

}



void LightSensorTouchLine()    //讓bot的兩個light往前推進一小步，直至壓線

{

     tick=CurrentTick();

     while(CurrentTick()     {

         OnFwdSync(Motors,25,0);

     }

}



void TireTouchLine(int n)     //讓bot往前移動，直至前輪壓線

{

         tick=CurrentTick();

         while(CurrentTick()         {

            OnFwd(LMotor,25);

            OnFwd(RMotor,25);

         }

}

void BotRightTurn()    //讓bot右轉

{

         tick=CurrentTick();

         while(CurrentTick()         {

            OnFwd(LMotor,25);

            OnFwd(RMotor,-25);

         }

}



void BotLeftTurn()     //讓bot左轉

{

         tick=CurrentTick();

         while(CurrentTick()         {

            OnFwd(LMotor,-25);

            OnFwd(RMotor,25);

         }

}





void BotTurning()

{

     Off(Motors);

     Wait(1000);

     LightSensorTouchLine();

     Off(Motors);

     Wait(1000);

     

     light1_value=Sensor(LightSensor1Port);

     light2_value=Sensor(LightSensor2Port);

     colornum=SensorHTColorNum(ColorSensorPort);

     

     if(light1_value>light2_value)  //可右轉

     {

         TireTouchLine(500);

         BotRightTurn();

         D=D+1;

         if(D>4) D=1;

     }

     if(light1_value<=light2_value) //可左轉

     {

         if(colornum==black_tap)    //彩色感應器偵測到前方有黑線，可前行

         {

             BotWalking();

             D=D;

         }

         if(colornum!=black_tap)

         {

             TireTouchLine(1400);

             BotLeftTurn();

             D=D-1;

             if(D<1) D=4;

         }

     }

}



void AxisRecord()

{

     if(D==1)

     {

         y=y+1;

     }

     

     if(D==2)

     {

         x=x+1;

     }



     if(D==3)

     {

         y=y-1;

     }

     

     if(D==4)

     {

         x=x-1;

     }

     str_x=NumToStr(x);

     str_y=NumToStr(y);

     str_log=str_x+" "+str_y;

     WriteLnString(fileHandle,str_log,bytesWritten);

}



void IfEndTask()

{

     Off(Motors);

     tick=CurrentTick();

     while(CurrentTick()     {

          PlayTone(880,20);

          Wait(450);

     }

     AxisRecord();

     StopAllTasks();

}





task main()

{

    DeleteFile("DataLog.txt");

    CreateFile("DataLog.txt",10000,fileHandle);



    SetSensorLowspeed(ColorSensorPort);

    SetSensorLight(LightSensor1Port,true);

    SetSensorLight(LightSensor2Port,true);

    InitialLightSensor();

}



task BotWalkTask()

{

     Follows(main);



     while(true)

     {

        colornum=SensorHTColorNum(ColorSensorPort);

        if((colornum!=blue_tap1)||(colornum!=blue_tap2))   //判斷是否遇到叉路

        {

            if(colornum==red_tap)

            {

               IfEndTask();

            }

            BotWalking();

        }

        if((colornum==blue_tap1)||(colornum==blue_tap2))

        {

            AxisRecord();

            BotTurning();



        }

     }

}

三、執行結果:

使用Robolab設計-直角轉彎軌道的line follower

一、硬體：

1.NXT主機輪形機器人
2.感測器：Light sensor×2、Ultrasonic sensor×1


二、軟體：
Robolab 2.9.4C


三、軌跡地圖：

四、程式碼：

五、示範影片：

簡單的line_follower設計

一、nxt機器人要做line follow，考慮用robolab設計時，我使用了2顆nxt light sensor做為感測元件，裝置的位置如下：

2個感測元件在黑色電工膠帶的兩側白色區域。

二、程式設計原理：

1.使用task split的方式，讓sensor-2和sensor-3獨立判讀

2.當sensor-2偵測在白色區域：Motor-B 前進（forward）

　當sensor-2偵測在黑色區域：Motor-B後退（Backward）

3.當sensor-3偵測在白色區域：Motor-C 前進（forward）

　當sensor-3偵測在黑色區域：Motor-C後退（Backward）

三、本程式僅對單純的直角轉彎（沒有叉路）的應用，可有良好的表現。

四、程式碼：