玩玩機器人，設計創意無窮

2012年2月17日星期五

Matlab解聯立方程式

matlab功能強大，對於工程解題更是他的強項。多年來，我對matlab總有一份眷戀，畢竟真的很好用。

ok，回到正題，因想到解聯立解，所以找一找網路上對matlab解聯立解的方法，找到一個方法還不錯用:

假設一個聯立方程式:
x+y=2.....(1)
x-y=0.....(2)

以觀察法可以判斷找到x=1,y=1為其解。

而matlab的程式表示方法，假設等號左邊的x,y未知數之係數，看成一個矩陣A，且等號右邊的係數可以看成一個陣列C
簡化上述的表示式，以矩陣可以寫成
AX=C

所以:
A=[1　1
　　1　-1]
C=[2
　　0]

底下是matlab程式的寫法：
A=[1 1;1 -1];
C=[2;0];
X=A\C

以左除法，即可解出聯立解，其中X得到的答案就是x,y的解。

2012年2月1日星期三

App Inventor 改版後的編輯網站

http://experimental.appinventor.mit.edu/

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

[執行結果]

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();



        }

     }

}

三、執行結果:

2011年11月24日星期四

Google App Inventor: 亂數產生器

亂數產生器，隨機產生一個或一組數字，在教室情境中很受歡迎。
當老師想要點名，卻又不想流於形式，亂數產生器得到的號碼，讓學生無法得知下一刻是誰被點到，那種急速上升刺激與快感，難以形容。

曾在Market找相關的軟體，卻未盡滿足我的需要，所以心中浮現用app inventor來開發，相信難度不高，且能客製化自己的需要來安排各種功能。

首先，我的軟體介面如下:

(圖1 軟體介面)

圖1中:
1.[亂數範圍]可由使用者自行決定起訖範圍。
2.[剔除不要的號碼]欄位，由使用者自行決定亂數產生時，以不出現此欄位指定的號碼為主。
3.當按下[隨機產生號碼]按鈕，即產生斗大的一個號碼於畫面中。

程式碼:

上述程式中:
1.StartNum及EndNum變數，分別指定為主畫面使用者輸入起訖號碼
2.RndNo變數，由[random integer函式]，隨機產生一數字並指定回RndNo，範圍如上述1.
3.RemoveElementNum變數，使用者指定[剔除號碼]後，系統自動統計其個數，並指定給RemoveElementNum
4.for range迴圈:
檢視[隨機產生的數字]是否與[欲剔除的數字]相等?若相等，則重新產生一個新的數字。直至不重複為止。

2011年11月19日星期六

Hitechnic Color Sensor 測試

一、硬體方面

color sensor 接在nxt主機上的IN_4。

二、軟體方面

根據［Third-party sensors: a review of some Hitechnic’s sensors］一文提到：
The new and totally redesigned HiTechnic Color Sensor Version 2 (V2) operates by using a single white LED (light emitting diode) to illuminate the target and analyses the color components of the light reflected by the target’s surface and calculates a Color Number that is returned to the NXT program.
Sensor Register Layout
Adress Type Contents
42H byte Color number
43H byte Red reading
44H byte Green reading
45H byte Blue reading
Procedures are already available in Bricxcc to use the sensor:
SetSensorLowspeed() initializes the I2C communication with the sensor.
SensorHTColorNum(port) returns the value of Color number.
ReadSensorHTRawColor(port, rouge, vert, bleu) returns the RGB readings.
ReadSensorHTColor(port, index, rouge, vert, bleu) returns the value of Color number, and the RGB readings. See the code example at the end of the article ...

Adress	Type	Contents
42H	byte	Color number
43H	byte	Red reading
44H	byte	Green reading
45H	byte	Blue reading

所以在NXC的程式基本採用的指令，包含二個:
1.　SetSensorLowspeed(IN_4)
2.　ReadSensorHTColor(IN_4,colorvalue,r,g,b)
其中，IN_4是彩色感應器接向IN_4，而colorvalue是經由color sensor回應的讀值，r,g,b分別為red,green,blue三原色的數值。

三、程式碼

#define ColorSensorPort IN_4

task main()
{

 byte r,g,b;
 byte colorvalue;
 SetSensorLowspeed(ColorSensorPort);

 while(true)
 {
     ReadSensorHTColor(ColorSensorPort, colorvalue,r,g,b);
     NumOut(0,LCD_LINE3,colorvalue);
     if (colorvalue==0)  PlayTone(880,25);
     Wait(25);
     ClearScreen();
 }
}

上述程式是說:
當color sensor讀值(colorvalue)=0，則PlayTone一個短音。
colorvalue=0-->黑色

訂閱：文章 (Atom)

玩玩機器人，設計創意無窮

買書捐殘盟

2012年2月17日星期五

Matlab解聯立方程式

2012年2月1日星期三

App Inventor 改版後的編輯網站

2011年12月16日星期五

走迷宮找最短路徑至終點

2011年12月9日星期五

篩選簡化路徑的Line Follower

2011年12月3日星期六

具路徑紀錄之Line Follower

2011年11月24日星期四

Google App Inventor: 亂數產生器

2011年11月19日星期六

Hitechnic Color Sensor 測試

友情連結

薛老師簡介

買書捐殘盟

2012年2月17日 星期五

2012年2月1日 星期三

2011年12月16日 星期五

2011年12月9日 星期五

2011年12月3日 星期六

2011年11月24日 星期四

2011年11月19日 星期六

2012年2月17日星期五

2012年2月1日星期三

2011年12月16日星期五

2011年12月9日星期五

2011年12月3日星期六

2011年11月24日星期四

2011年11月19日星期六