撲克牌洗牌機

久違了，好久沒有新的idea，今天寫一篇關於利用NXT做撲克牌發牌機。

基本上，把撲克牌放兩邊，模擬我們人的手洗牌的樣子。
而機構本身利用輪胎來推進撲克牌至中間的凹槽，馬達轉動的速度無需過快，以免撲克牌卡紙而破壞了牌紙。

且程式也止不過幾行，其中主要用以啟動馬達轉動的程式碼:

OnFwd(OUT_AB,50);

以下是洗牌機的影片片段:

走迷宮找最短路徑至終點

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

[執行結果]

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

具路徑紀錄之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();



        }

     }

}

三、執行結果:

採用滑動模式設計，應用於NXT馬達定位控制

根據前述（線性非時變，可變結構滑動模式的概念），茲嘗試建立滑動模式控制應用於NXT馬達定位控制，結果定位效果顯著且迅速。

相關參數簡述如下：
（一）滑動曲面函數S(X)=0.7X1+X2=0
　　　其中X1=期望值-實際量測值，X2=X1變化率
（二）當S‧X1>0，提供一正向驅動控制量，本例為Ud=2*err;
　　　當S‧X1<0，提供一正向驅動控制量，本例為Ud=-2*err;
　　　滑動模式控制量Ueq=-0.5*err;
（三）總驅動控制量U=Ud+Ueq
（四）紀錄X1、X2及馬達回應的encoder轉動值，儲存至NXT主機中（本例指定為DataLog.txt）。

[執行結果]



左圖為定位控制的觀測值，期望值為180度。由圖中可看出定位速度很快。













左圖為x1,x2的平面軌跡圖。
迅速滑向原點。















上述參數可由使用者自行更改，以求較佳控制，筆者僅略以此控制法則說明滑動模式非常適用。

[原始程式]

task main()
{
   ResetRotationCount(OUT_A);
   long readvalue=0;
   long DesirePosition=180;
   long err=0,prev_err=0;;
   long cerr=0;
   float S=0;
   float Ud=0,Ueq=0;
   int U=0;
  
   byte fileHandle;
   short bytesWritten;
   DeleteFile("DataLog.txt");
   CreateFile("DataLog.txt",10000,fileHandle);
   string str_position;
   string str_err;
   string str_cerr;
   string str_log;
  
   while(!ButtonPressed(BTNCENTER,true))
   {
      str_position=NumToStr(readvalue);
      str_err=NumToStr(err);
      str_cerr=NumToStr(cerr);
      str_log= str_position+"  " +str_err+ "  " +str_cerr;
      WriteLnString(fileHandle,str_log,bytesWritten);
  
      readvalue=MotorRotationCount(OUT_A);
      err=DesirePosition-readvalue;
      cerr=(err-prev_err);
     
      S=0.7*(err)+(cerr);
      prev_err=err;
     
      if(S*err>0)
      {
          Ud=2*err;
      }
      if(S*err<0)        {            Ud=-2*err;        }        Ueq= -0.5*err;                U=Ud+Ueq;                if(U>100) U=100;
      if(U<-100) U=-100;         OnFwd(OUT_A,U);        TextOut(0,LCD_LINE1,"degree=");        NumOut(6*8,LCD_LINE1,readvalue);        Wait(50);        Off(OUT_A);        ClearScreen();     }     CloseFile(fileHandle); } 

Runge-Kutta應用於NXT-兩輪平衡車角度的量測

陀螺儀對機身擾動非常敏感，在前述透過卡爾門濾波器的設計，可以有效減低它的敏
感性。

目前進行陀螺儀對二輪平衡車測定其傾斜機身角度，運用離散時間之轉角速度與時間
的乘積加總所得的角度，會有誤差的問題之外，陀螺儀在運作期間，也會因為飄移的問題
，使角度誤差的情況更加嚴重（換句話說：當二輪平衡車在前傾與後仰的維持平衡一
段時間後，當機身回到直立狀態的某一瞬間，其角度並非零值，可能為負或正），此
刻易使控制器產生誤判而使機身搖擺。

所以程式中，另行加入Runge-Kutta的二階微分技術，除了使測定的角度更加精準之
外，尚須扣除角度的偏移值（bias），將之應用在NXT，明顯改善本問題。

[NXC版本]

#define GyroScopePort IN_3
#define sample_size 100
#define GyroScale 4
#define t_scale 1e+6

//Define gyro offset parameters
float gyro_sum=0, gyro_avg=0;
int gyro_value=0;
int offset=0;
int i;

//Define Kalman filter parameter
float P=10;  //Process state covariance
float Kgain;        //The Kalman Filter Gain
float X_old=0;
float X_curr;
float Q=0.0001;     //The Probability of process noise w ,P(w)~N(0,Q)
float R=0.1;        //The Probability of measurement noise v, P(v)~N(0,R)

//Define BodyTiltTheta parameters
float theta=0,prev_theta=0;
float f1=0,f2=0;
float aa=0.001;
float theta_bias=0;
long prev_tick;


int KalmanFilter(int gyro_value)
{
X_curr=X_old;
P=P+Q;
Kgain=P/(P+R);
X_curr=X_curr+Kgain*(gyro_value-X_curr);
P=(1-Kgain)*P;
X_old=X_curr;
return (X_curr);
}


void CalcGyroOffset()
{
for(i=0;i<sample_size;i++)
{
   gyro_value=SensorHTGyro(GyroScopePort);
   gyro_sum+=gyro_value;
   Wait(4);
}
gyro_avg=gyro_sum/sample_size;
offset=gyro_avg+0.5;
}

void ShowLCD()
{

 TextOut(0,LCD_LINE1,"theta=");
 NumOut(6*6,LCD_LINE1,theta-theta_bias);
 Wait(20);
 ClearScreen();
}


task main()
{
SetSensorHTGyro(GyroScopePort);

CalcGyroOffset();
prev_tick=CurrentTick();
while(true)
{
  gyro_value=SensorHTGyro(GyroScopePort,offset);
  f2=KalmanFilter(gyro_value)/GyroScale;
  Wait(1);
  theta=prev_theta+(f1+2*f2)*(CurrentTick()-prev_tick)/t_scale;
  prev_theta=theta;
  gyro_value=SensorHTGyro(GyroScopePort,offset);
  f1=KalmanFilter(gyro_value)/GyroScale;
  prev_tick=CurrentTick();
  theta_bias=(1-aa)*theta_bias+aa*theta;
  ShowLCD();
 }
}

乒乓球發球機

有一兩個月沒有特別去做樂高的作品。其實一方面對於賽格威（segway）的題目還一直想要嘗試去改善車子的穩定度，另一方面也在尋找新的主題，而且賽格威當時組裝起來也夠麻煩，拆掉又挺可惜，所以就一直沒有動它。:p

最近看WRO（世界機器人大賽組織）的比賽，對於機器人投籃比賽感到興趣，所以手癢也想要組裝一台來玩玩。而組裝的時候，突然想到：如果能讓發球機一直作投球的動作，那麼就可以讓打乒乓球的人，可以作練球的動作。

一、機構的外觀圖：

以下是目前所做的成果：

990712-Segway再現

一直對Segway兩輪平衡車設計不出來耿耿於懷。話說Segway兩輪平衡車我打從著手設計程式至今，大概有5個多月了吧。讀了很多相關的文章，從陀螺儀的理論、陀螺儀的定位方法、研究所的相關論文；也使用了Robolab、Labview以及現在使用的Bricxcc等工具，現在稍稍有一點點頭緒。

不過，目前還有缺點：

1.車子往後傾斜，車輪不會動？

2.車子在行駛時，在數秒後車輪會有搖擺的現象（影片中可以看到），之後碰到障礙物後，就直接倒下。

以上都是待改進之處。

程式碼：

#define GyroPort IN_3

#define dt 0.001

#define GyroScale 4

#define t_scale 500

int flag=0;

long GyroBiasVal=0;

long curr_time=0;

long GyroCounter=0;

float bias=0.0000;

float theta=0,pre_theta=0,theta_bias=0;

float f1=0,f2=0;

long t_old=0;

float power=0;

float x=0,pre_x=0,dot_x=0;

float aa=0.001;

float K1=0,K2=90,K3=8,K4=10,V=0.195,K_pwr=0;

void initGyro(){

　curr_time=CurrentTick();

　PlayTone(440,50);

　while(CurrentTick()<(3000+curr_time)){ //3秒內，計算gyro bias

　　GyroBiasVal=GyroBiasVal+SensorRaw(GyroPort);

　　TextOut(0,LCD_LINE5,"Computing Bias..");

　　Wait(20);

　　GyroCounter++;

　}

　PlayTone(880,50);

　bias=GyroBiasVal/GyroCounter;

}//End of initGyro Function

void get_body_theta(){

　f2=(SensorRaw(GyroPort)-bias)/GyroScale;

　Wait(1);

　theta=pre_theta+(f1+2*f2)*(CurrentTick()-t_old)/t_scale;

　pre_theta=theta;

　f1=(SensorRaw(GyroPort)-bias)/GyroScale;

　t_old=CurrentTick();

　x=MotorTachoCount(OUT_B);

　dot_x=x-pre_x;

　pre_x=x;

　theta_bias=(1-aa)*theta_bias+aa*theta;

}//End of get_body_theta ,use Runge Kutta method order 2

task main(){

　SetSensorHTGyro(GyroPort);

　initGyro();

　ClearScreen();

　t_old=CurrentTick();

　ResetTachoCount(OUT_BC);

　//↓Gain computing based on Battery Voltage

　K_pwr=0.7+(0.7-1.1)/(8816-8196)*(BatteryLevel()-8816);　

　while(true){

　　get_body_theta();

　　power=K_pwr*(K1*x+K2*(dot_x-V)+K3*(theta-theta_bias)+K4*(f1));

　　if(power>100) power=100;

　　if(power<-100) power=-100;

　　SetOutput(OUT_BC, Power, power, OutputMode,OUT_MODE_MOTORON,

　　　　　　　RunState,OUT_RUNSTATE_RUNNING,

　　　　　　　UpdateFlags,UF_UPDATE_MODE+UF_UPDATE_SPEED);

}

}//End of Task Main

==================

執行結果：

參考文獻：

1.wiki-nxt_segway-Jean-Philippe Panaget's website

2.以陀螺儀 (HiTechnic Gyro)推算物體的旋轉角度的實作測試

3.Balance Robot

4.Self-Balancing Segway NXTway-Gyro bot

5.兩輪平衡車-樂高機器人

6.A PID Controller For Lego Mindstorms Robots.

7.NXTway-GS Building

8.NXTway Model-Based

9.DSP主控之兩輪機器人平衡與兩輪同步控制，蔡僑倫，國立中央大學-93.06

10.以樂高NXT 實作智慧型兩輪平衡車，羅嘉寧等，銘傳大學-2009資訊科技與實務國際研討會

11.Hitechnic Sensor Website

12.數值分析Runge-Kutta Method order 2,4

13.Kalman Filter