前一篇的文章[
篩選簡化路徑的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根據電腦鼠依現在座標、下一個駐點座標的二者地理位置座標，決定左右轉或前行，方向由使用者定義方向，於程式中設定之。
[執行結果]
為大圓距離， 
是地球半徑(6371km)，而點 1 經度、緯度分別為 
、 
，點 2 的經度、緯度分別為 
、 
。
