走迷宮找最短路徑至終點

前一篇的文章[篩選簡化路徑的Line Follower]談到:

『電腦鼠從起點到終點所經過的所有途徑，記錄所經過的座標之後，自動篩選及簡化路徑，進而尋找最短途逕從起點直至終點。』

本篇據此，經由nxt分析 最短途徑後，操作者將nxt電腦鼠再次放到起點處，預備讓電腦鼠可以依據最短途逕直至終點。不過程式需要增加一段，也是本篇的精華所在。

#define blue_tap1 3
#define blue_tap2 4
#define red_tap 9
#define factor 2


#define LMotor OUT_A
#define RMotor OUT_C
#define Motors OUT_AC

#define LightSensor1Port IN_1
#define LightSensor2Port IN_2
#define ColorSensorPort IN_4

int x[10];
int y[10];
int x_fix[10];
int y_fix[10];

int i,j,k=0;
int D;
int colornum;
int black,white,light1_value,light2_value;
int Lpower=0,Rpower=0;

long tick;
string code;


void WaitSEC3()
{
   tick=CurrentTick();
   while(CurrentTick()＜(tick+3000))
   {
       PlayTone(880,50);
       Wait(950);
   }
   Wait(1000);
}

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();
   WaitSEC3();
}


void LoopAdjust()
{
   for(i=0; i＜ArrayLen(x); i++)
   {
        x_fix[k]=x[i];
        y_fix[k]=y[i];
        for(j=i; j＜ArrayLen(x); j++)
        {
              if((x[j]==x[i])&&(y[j]==y[i]))
              {
                  x_fix[i]=x[j];
                  y_fix[i]=y[j];
                  i=j;
              }
        }
        k++;
   }
}

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()＜tick+800)
    {
        OnFwdSync(Motors,25,0);
    }
}

void TireTouchLine(int n)     //讓bot往前移動，直至前輪壓線
{
        tick=CurrentTick();
        while(CurrentTick()＜tick+n)
        {
           OnFwd(LMotor,25);
           OnFwd(RMotor,25);
        }
}

void BotRightTurn()    //讓bot右轉
{
        LightSensorTouchLine();
        Off(Motors);
        Wait(SEC_1);
        TireTouchLine(500);
        tick=CurrentTick();
        while(CurrentTick()＜tick+2050)
        {
           OnFwd(LMotor,25);
           OnFwd(RMotor,-25);
        }
}

void BotLeftTurn()     //讓bot左轉
{
        LightSensorTouchLine();
        Off(Motors);
        Wait(SEC_1);
        TireTouchLine(1400);
        tick=CurrentTick();
        while(CurrentTick()＜tick+1600)
        {
           OnFwd(LMotor,-25);
           OnFwd(RMotor,25);
        }
}




void  ShortLoopGo()      //Result: (0,1)-->(-1,1)-->(-1,0)-->(-2,0)
{
   string prev_axis="Y" ;
   i=1;

   while(i＜=k)
   {
       colornum=SensorHTColorNum(ColorSensorPort);
       //NumOut(0,LCD_LINE1,D);
       if((colornum!=blue_tap1)||(colornum!=blue_tap2))
       {
            if(colornum==red_tap)
            {
                StopAllTasks();
            }
            BotWalking();
       }
      
       if((colornum==blue_tap1)||(colornum==blue_tap2))
       {
            Off(Motors);
            Wait(SEC_1);
            if((x_fix[i]-x_fix[i-1])!=0)
            {
                if(((x_fix[i]-x_fix[i-1])>0)&&(prev_axis=="Y"))
                {
                   D=1;
                }
                if(((x_fix[i]-x_fix[i-1])>0)&&(prev_axis=="-Y"))
                {
                   D=2;
                }

                if(((x_fix[i]-x_fix[i-1])＜0)&&(prev_axis=="Y"))
                {
                   D=3;
                }
                if(((x_fix[i]-x_fix[i-1])＜0)&&(prev_axis=="-Y"))
                {
                   D=4;
                }

                if(((x_fix[i]-x_fix[i-1])>0)&&(prev_axis=="-X"))
                {
                   D=5;
                }
                if(((x_fix[i]-x_fix[i-1])＜0)&&(prev_axis=="X"))
                {
                   D=6;
                }
            }
           
            if((y_fix[i]-y_fix[i-1])!=0)
            {
                if(((y_fix[i]-y_fix[i-1])>0)&&(prev_axis=="X"))
                {
                   D=7;
                }
                if(((y_fix[i]-y_fix[i-1])>0)&&(prev_axis=="-X"))
                {
                   D=8;
                }

                if(((y_fix[i]-y_fix[i-1])＜0)&&(prev_axis=="X"))
                {
                   D=9;
                }
                if(((y_fix[i]-y_fix[i-1])＜0)&&(prev_axis=="-X"))
                {
                   D=10;
                }

                if(((y_fix[i]-y_fix[i-1])>0)&&(prev_axis=="Y"))
                {
                   D=11;
                }
               
                if(((y_fix[i]-y_fix[i-1])＜0)&&(prev_axis=="-Y"))
                {
                   D=12;
                }
            }
            NumOut(0,LCD_LINE1,D);
            switch(D)
            {
                case 1:
                   BotRightTurn();
                   prev_axis="-X";
                   break;
                case 2:
                   BotLeftTurn();
                   prev_axis="-X";
                   break;
                case 3:
                   BotLeftTurn();
                   prev_axis="X";
                   break;
                case 4:
                   BotRightTurn();
                   prev_axis="X";
                   break;
                case 5:
                   LightSensorTouchLine();
                   Off(Motors);
                   Wait(SEC_1);
                   TireTouchLine(500);
                   BotWalking();
                   prev_axis="-X";
                   break;
                case 6:
                   LightSensorTouchLine();
                   Off(Motors);
                   Wait(SEC_1);
                   TireTouchLine(1400);
                   BotWalking();
                   prev_axis="X";
                   break;
                case 7:
                   BotRightTurn();
                   prev_axis="Y";
                   break;
                case 8:
                   BotLeftTurn();
                   prev_axis="Y";
                   break;
                case 9:
                   BotLeftTurn();
                   prev_axis="-Y";
                   break;
                case 10:
                   BotRightTurn();
                   prev_axis="-Y";
                   break;
                case 11:
                   LightSensorTouchLine();
                   Off(Motors);
                   Wait(SEC_1);
                   TireTouchLine(500);
                   BotWalking();
                   prev_axis="Y";
                   break;
                case 12:
                   LightSensorTouchLine();
                   Off(Motors);
                   Wait(SEC_1);
                   TireTouchLine(1400);
                   BotWalking();
                   prev_axis="-Y";
                   break;
            }
            i++;
       }
   }
   Off(Motors);
}

task main()
{
   SetSensorLowspeed(ColorSensorPort);
   SetSensorLight(LightSensor1Port,true);
   SetSensorLight(LightSensor2Port,true);
   ArrayBuild(x,0,-1,-1,-1,-2,-2,-2,-1,-1,-2);
   ArrayBuild(y,1, 1, 2, 3, 3, 2, 1, 1, 0, 0);
   LoopAdjust();
   InitialLightSensor();
   while(!ButtonPressed(BTNCENTER,false));
   ClearScreen();
   ShortLoopGo();
}

●★●★●★●★●★●★●★●★●★●★
程式說明
1.LoopAdjust()函式-用以調整簡化最短路徑
2.BotWalking()函式-電腦鼠走黑線的副函式
3.LightSensorTouchLine()、TireTouchLine()-讓電腦鼠光感、輪胎在轉彎處修正其位置
4.BotRightTurn()-電腦鼠右轉副函式
5.BotLeftTurn()-電腦鼠左轉副函式
6.ShortLoopGo()-最短路徑副函式－本篇文章核心函式（Kernel Sub-Function）
(1)根據電腦鼠座標位置，於叉路轉彎，共有12個條件式
(2)依現行軌道，從起點處向上（或向前）Y為正，向下（或向後）Y為負；
(3)從起點處向左X為正，向右為負。
(4)前述2,3根據電腦鼠依現在座標、下一個駐點座標的二者地理位置座標，決定左右轉或前行，方向由使用者定義方向，於程式中設定之。

[執行結果]

篩選簡化路徑的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();



        }

     }

}

三、執行結果: