買書捐殘盟

2011年12月16日 星期五

走迷宮找最短路徑至終點

前一篇的文章[篩選簡化路徑的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根據電腦鼠依現在座標、下一個駐點座標的二者地理位置座標,決定左右轉或前行,方向由使用者定義方向,於程式中設定之。

[執行結果]

video

2011年12月9日 星期五

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

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