買書捐殘盟

2011年12月3日 星期六

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

}
}
}


三、執行結果:

video

沒有留言:

張貼留言