具路徑紀錄之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();



        }

     }

}

三、執行結果: