樂高 Lego 與 Arduino 機器人

2010年12月21日星期二

NXT 軌跡車馬達 PID 控制

在做NXT軌跡車設計時如果路線是轉彎度平滑筆直的時候NXT走得很順利，但如果把路線的轉彎角度變大時會發現NXT有時會偏離路線走失了。

這是因為NXT左右兩顆驅動馬達的反應速度跟不上路線的變化導致無法即時修正，這時候就是馬達PID控制可以發揮所長的地方，因為除了比例P模式之外還有微分模式、積分模式可以更快速反應路線的變化資訊給NXT做必要的修正。

比例P模式：P(t) = E(t)   LightSensor與Motor電力比例值
積分I 模式：I(t) = I(t-1) + E(t)   每次讀取比例值的累積
微分D模式：D(t) = E(t) – E(t-1)    LightSensor 前次與本次差異值累積

這三個參數的調整方式都有不一樣的效果建議大家試試看，如何走的順又平穩就是最佳的參數值，像影片中的走法還有改進的空間雖然LightSensor一直維持在黑線中間但電力的供應還不夠平順。

2010年12月16日星期四

NXC 指令：ColorSensor

NXT2.0 有一個新的感應器 ColorSensor 可以辨識6種顏色

Color Define：

Black
Blue
Green
Yellow
Red
White

NXC也可以控制ColorSensor 的燈光顏色（Red、Green、Blue、White）

用TouchSensor來操控ColorSensor的燈光顏色，每按一下就換一種燈光顏色

Example：
task main(){
   int Light=0;
   SetSensorTouch(S2);
    while(true){

if(Sensor(S2)==1){

Light++;

Wait(300);

}

switch(Light){

case 1:

SetSensorColorRed(S3);

break;

case 2:

SetSensorColorGreen(S3);

break;

case 3:

SetSensorColorBlue(S3);

break;

case 4:

SetSensorColorFull(S3);

Light=0;

break;

}
NumOut(20, LCD_LINE3, Sensor(S3)) //讀取ColorSensor的數值

}

NXC 指令：關掉Sensor功能

有網友在探奇向邱老師提出一個問題「如何關掉超音波 Sensor 的功能」

一開始我覺得把SetSensorLowspeed(S1) 改為 SetSensorLowspeed(NULL) 應該就可以了但實際測試失敗，我查了一下NXC 的 Help 發現Sensor_TYPE_ 裡面有SENSOR_TYPE_NONE，所以改為SetSensorType(S1,SENSOR_TYPE_NONE) 就可以關掉特定Port的Sensor Function。

反過來想到底何時會想關掉特定Sensor功能呢???我覺得至少在電力方面應該有其需要....^_^

樂高網路影片分享

NXTCam 的運作範例

NXC 指令：Precedes

Precedes：啟動多個Task

void Precedes( task1 , task2 ,task3 ,.....taskN)

用途：可同時啟動多個Task各自執行自己的程序。

StopAllTasks()：停止所有Task
StopTask(taskA)：停止taskA的程序
Stop(x == 24); // stop the program if x==24

Ex：假設有兩個Task 都對MotorA下指令一個是前進另一個是後退，這時你會看到NXT MotorA有時前進有時後退變得無所適從。

如果有多個Task對同一個驅動馬達下指令時就看誰先搶到工作權誰就先執行，因此這時候在使用Precedes指令時最好用mutex 來判斷誰最需要「工作權」，有點類似十字路口的紅綠燈，假如路口沒有號誌燈那所有車輛都要前進時就會發生車禍或是某個方向的車子一直前進而另一個方向的車子卻動彈不得，因此mutex 就像號誌燈讓多個方向的車流都能維持一定的流量而不打結。

從以下Example你可在NXT 螢幕看到反覆顯示   「go1」 0.5秒「go2」

範例：

mutex muxFlag;
task go1(){
    while(true){
       Acquire(muxFlag);
       TextOut(10,LCD_LINE3,"go1",false);
       Wait(500);
       ClearScreen();
       Release(muxFlag);
    }
}

task go2(){
    while(true){
       Acquire(muxFlag);
       ClearScreen();
       TextOut(10,LCD_LINE4,"go2",false);
       Wait(500);
       ClearScreen();
       Release(muxFlag);
    }
}

task main(){
    Precedes(go1,go2);
}

NXC 指令：Follows

Follows：一直等到設定的Task執行完畢，才會繼續執行下一個Task

用途：當 NXT 執行工作有固定順序時可以用 Follows 控制，這跟Precedes同時執行多個Task有不同的用法。

void Follows( task1 , task2 ,task3 ,.....taskN)

從以下Example你可在NXT 螢幕看到依序顯示「Main」 5秒「go1」 5秒「go2」

範例：

task main(){
    ClearScreen();
    TextOut(10,LCD_LINE2,"Main",false);
    Wait(5000);
}

task go1(){
    Follows(main);
    ClearScreen();
    TextOut(10,LCD_LINE3,"go1",false);
    Wait(5000);
}

task go2(){
    Follows(go1);
    ClearScreen();
    TextOut(10,LCD_LINE4,"go2",false);
    Wait(5000);
}

2010年12月13日星期一

LEGO NXT 藍牙遙控車

樂高有一款控制器 LEGO Power Functions IR Remote Control 加上HiTechnic 公司有個產品 IR Receiver Sensor 可當NXT的接收器，你就能像一般遙控玩具車一樣遙控你的NXT。

因為我有2台NXT所以我是利用藍牙連線互傳資料的方式來遙控，一台Master當遙控器發射訊息而另一台當Slave接收訊息後再對2顆馬達進行操控，因此一顆馬達控制左右轉向另一顆是控制前進後退。

車體的樣式組裝我是用 HiTechnic 的範例
材料：

Lego MindStorms NXT
驅動馬達Motor 2顆
車體結構組件

程式不是寫的很完備還有很多地方要調整，但控制車體的轉向與前進後退功能都有，有興趣的人可以試試看，另外如果藍牙手機有NXT控制程式就可以用手機當遙控器使用。

我覺得這個藍牙遙控車最主要是讓我們學習與了解2個NXT要如透過藍牙溝通傳遞訊息，這樣可以幫助我們往後設計更大型的樂高物件時會更有變化與多樣性。

結論：如果真的要當遙控車玩還是要買專用的Remote Control比較順手，但是要玩遙控車這樣的成本好像太高了。

在拍攝影片影片的時候，旺弟弟正在練習直笛所以背景有直笛音樂.....^_^

程式方面如下：

//Master：遙控器程式

#define BT_CONN 1
#define INBOX1 1
#define OUTBOX1 1
#define INBOX2 2
#define OUTBOX2 2

sub BTCheck (int conn){

    if (!BluetoothStatus(conn)==NO_ERR){
        TextOut(5,LCD_LINE2,"Error");//顯示連線錯誤訊息
        Wait(1000);
        Stop(true);
    }
}

task main(){

        BTCheck (BT_CONN); //檢查與slave之連線
        int rev=0, rev1=0,sed=0,sed1=0,sed2=0,pwr1=0,pwr2=0;

        while(true){
            //左右轉向馬達
            sed=MotorTachoCount(OUT_A)/20;
            if(sed!=sed1){
                pwr1=sed1-sed;
                sed1=sed;
                SendRemoteNumber(BT_CONN, OUTBOX1,pwr1);
            }

            //前進後退馬達
            sed=MotorTachoCount(OUT_B)/20;
            SendRemoteNumber(BT_CONN, OUTBOX2,sed2);
            if(sed!=sed2){
                pwr2=sed2-sed;
                sed2=sed;
                SendRemoteNumber(BT_CONN, OUTBOX2,pwr2);
            }
          }
}

------------------------------------------------------------------------

//Slave：車體接收遙控程式
#define INBOX1 1
#define OUTBOX1 1
#define INBOX2 2
#define OUTBOX2 2

sub BTCheck(int conn){
    if (!BluetoothStatus(conn)==NO_ERR){
        TextOut(5,LCD_LINE2,"Error"); //顯示連線錯誤訊息
        Wait(1000);
        Stop(true);
        }
}

task main(){
    BTCheck(0); // 從slave來看，master都是連接在line 0
    int rev=0, rev1=0, rev2=0,sed=0,sed1=0,rev3=0,rev4=0;

    while(true){

        ReceiveRemoteNumber(INBOX1, true, rev3);
        if(rev3!=rev1){
            rev1=rev3;
            ClearScreen();
            if(rev1>0)
              OnFwd(OUT_A,50);
            if(rev1<0)
              OnFwd(OUT_A,-50);
            Wait(200);
            Off(OUT_A);
        }

        ReceiveRemoteNumber(INBOX2, true, rev4);
        if(rev4!=rev2){
            rev2=rev4;
           if(rev2>0)
              OnFwd(OUT_B,50);
            if(rev2<0)
              OnFwd(OUT_B,-50);
        }
    }
}//main

置頂文字

2010年12月21日 星期二

2010年12月16日 星期四

2010年12月13日 星期一

2010年12月21日星期二

2010年12月16日星期四

2010年12月13日星期一