買書捐殘盟

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

4 則留言:

  1. 這個主題是目前做的專題,搞最久的!
    電腦鼠走迷宮,向來是大專院校單晶片專題的首選。以前是用8051晶片在指定比賽的迷宮中搜尋,多年之後的現在,nxt也能讓想做電腦鼠的玩家過過癮。

    回覆刪除
  2. 打算據此為基礎,把Line Follower的軌道弄複雜一點,讓這一隻mouse去走真正的迷宮,看能不能走得出來! :p

    回覆刪除
  3. 薛老師你好:
    我覺得老師您好厲害喔,而目前我也有在製作電腦鼠相關專題
    所以我想請問一下老師,我的相關專題也是有記憶部分,但是皆以光感測器為主,因為想嘗試不要用貼色塊方式(無色彩感應器)來判斷,(都用光感應器),因為希望能直接將迷宮的節點座標的部分寫入程式(使用JAVA語言)不知是否OK?要如何讓機器人知道那裏有座標並能將走過的座標記憶下來?
    以及在記憶部分若使用螞蟻演算法OK?或是使用左手或右手定則?

    回覆刪除
  4. 貼色塊,主要用以分界座標,如果不貼色塊,也許可以考慮bot步行的距離來計算間距,不過是否會有誤差,必須實驗後方知。
    本來想說用gps sensor來做,但後來發現bot的馬達會干擾gps,所以就無後續的研究。
    螞蟻演算法,藉由費洛蒙的濃度來決定最短路徑,應該是可以做,您可以試看看!

    回覆刪除