乒乓球發球機

有一兩個月沒有特別去做樂高的作品。其實一方面對於賽格威（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

Runge Kutta Method order 4-Example 1

Example 1: solve dy/dx=f(x,y)=3exp(-x)-0.4y，y(0)=5，find y(3)=？

Ans:

使用Runge Kutta Order 4求近似解，已知Runge Kutta Method order 4的一般式：

dy/dx=f(x,y)

→y(k+1)=y(k)＋(1/6)×h×(k1+2k2+2k3+k4)

其中:

k1=f(x(k),y(k))

k2=f(x(k)+(1/2)h,y(k)+(1/2)k1×h)

k3=f(x(k)+(1/2)h,y(k)+(1/2)k2×h)

k4=f(x(k)+h,y(k)+k3×h)

h: 遞增步距

程式範例：

%dy/dx=f(x,y)=3exp(-x)-0.4y, y(0)=5

%Using Runge-Kutta order 4: y(k+1)=y(k)+1/6(k1+2k2+2k3+k4)h

clear all;

clc

h=0.1; %Step

L=0;

R=3;

X=0;Y=5;

counter=1;

for v=L+h:h:R

　k1=Func(X,Y);

　k2=Func(X+0.5*h,Y+0.5*k1*h);

　k3=Func(X+0.5*h,Y+0.5*k2*h);

　k4=Func(X+h,Y+k3*h);

　Y=Y+h*(k1+2*k2+2*k3+k4)/6;

　Y_sol=10*exp(-0.4*(X+h))-5*exp(-(X+h));

　err(counter)=Y_sol-Y;

　Yk(counter)=Y;

　Yk_sol(counter)=Y_sol;

　counter=counter+1;

　X=X+h;

　plot(Yk,'r-')

　hold on

　plot(Yk_sol,'b*')

end

hold off

副程式func.m

function k=Func(x,y)

k=3*exp(-x)-0.4*y;

end

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

執行結果：

h=0.1（分析步距）

原微分方程式的解y(x)=10e^-0.4x-5e^-x

上圖中，紅色線為期望值，藍色點是近似值。

利用RK4分析微分方程式的動態行為，其近似值逼近期望值。

總誤差=Σ(每一點的誤差)=1.3587×10^-6

梯形積分法

f(x)在封閉區間[a,b]求積分，若f(x)為複雜的函數， 對於求解積分，可用

數值分析的梯形積分法來求解。

梯形積分法，得到的結果，對照原函數定積分的值，可發現誤差值很小。所以被廣泛運用。

以下用matlab 語法，撰寫梯形積分的algorithm：

%Function F(x)=∫sin(x)dx，積分範圍0~π

a=0;

b=pi;

n=1000;

A=0;

for i=a:(b-a)/n:b

　　if i==a

　　　f1=sin(i);

　　else

　　　f2=sin(i);

　　　A=A+0.5*(f1+f2)*(b-a)/n;

　　　f1=f2;

　　end

end

disp('梯形積分法面積=');

A

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

經執行後，A＝2.0000

與積分範圍[0,pi]，∫sin(x)dx=2的結果一致。

Runge Kutta Method order 2

微分方程式y'=y； y(0)=1

Ans:

使用Runge Kutta Method order 2:

Runge-Kutta Method通式：

y'＝f(x,y)

x_i=x₀+ih,i=0,1,2…

y_k+1＝y_k＋(1/2)．(k₁+k₂)

其中

k₁=h．f(x_k,y_k)

k₂=h．f(x_k＋h,y_k＋k1)

假設h=0.5(漸進步距)

所以：

y'＝f(x,y)＝y

y(0)＝1，換句話說y₀＝1

x₁=x₀+i×h=0+1×0.5=0.5

k₁＝h×f(x₀,y₀)＝0.5×f(0,1)=0.5×1=0.5

k₂＝h×f(x₀＋h,y₀＋k₁)＝0.5×f(0＋0.5,1＋0.5)=0.5×f(0.5,1.5)=0.5×1.5=0.75

其中f(x,y)=y，帶入x=0,y=1進f(x,y)中，得f(0,1)=1

--> y₁＝y₀＋(1/2)×(k₁+k₂)=1+(1/2)×(0.5+0.75)=1.625

x₂=x₀+i×h=0+2×0.5=1.0

k₁＝h×f(x₁,y₁)＝0.5×f(0.5,1.625)=0.5×1.625=0.8125

k₂＝h×f(x₁＋h,y₁＋k₁)

＝0.5×f(0.5＋0.5,1.625＋0.8125)

＝0.5×f(1,2.4375)=0.5×2.4375=1.2188

-->y₂＝y₁＋(1/2)×(k₁+k₂)=1.625+(1/2)×(0.8125+1.2188)=2.6406

對正式解而言，前述y’=y，y(0)=1的通解是y=e^x：

1. y(0)=1

2. y(0.5)=1.6487，經由Runge-Kutta方法計算的y1=1.625，

誤差率＝(1.6487-1.625)/1.625=1.46%

3.y(1)=2.71828，經由Runge-Kutta方法計算的y2=2.6406，

誤差率＝(2.71828-2.6406)/2.6406=2.94%

NXT lego 機器人學術網站

YCChen 協作平台

這個網站很不錯，提供很多優秀的作品，甚至也有學術的相關研究。

值得參考！

網址：http://sites.google.com/site/ycchenster/home/xue-shu-yan-jiu/lego

NXT-自動路邊停車(Auto Parking Car)

作品說明：

一、硬體：

1.nxt主機

2.nxt-Motor×3

3.UltraSonic Sensor×1

4.馬達A控制車子後輪前進後退、馬達B控制車子超音波感測器的偵測方向、

馬達C控制車子前輪轉向（steering）

程式碼：

作品欣賞：

NXT_Clock

題目：

1.用 NXT 就可以設計一個電子鐘，平時正常顯示時間 HH:MM:SS，不做任何組裝。
2.按橘色鍵兩秒可以進入編輯模式，時間計時暫停，被編輯的數字閃爍，其它的數字仍然恆亮不閃爍。
3.再按橘色鍵可選擇小時、分鐘或秒數，第一次切換編輯小時，第二次能編輯分鐘，第三次可調整秒數，第四次回到小時編輯。
4.按橘色鍵兩秒則脫離編輯模式，恢復正常時間顯示。
5.左右方向鍵可將數值上下調整，調整時進位或退位不會影響到其它數字。

程式碼：

作品展示：

Labview for nxt: counter的設計

Labview有一個移位暫存器（Shift Register），看了書仍一直看不懂那個東西的使用意義。很感謝網友阿吉老師的指導，終於搞清楚了。

我試著做一個類似[i=i+1]的功能，其中shift register的作用，會記得前一次的值，做為下一次的引數。

Labview for nxt: variable 的使用

對於labview for nxt的變數應用，今天終於摸出一點頭緒了～ 稍做整理筆記如下：

備註：

1.設定變數前，先對目標（例如：light sensor）create indicator，使之產生一個指示器

2.在nxt toolkit的structure中，有一個local variable，把它拉到block diagram裡

3.在local variable上按右鍵，點選快速選單中「Select Item」，選擇其中一個Item當作變數（如本例中的Ref或Current）

4.在local variable上按右鍵，選「Change to Read」，便可使該變數隨時讀取該裝置的讀值。

NXT-MORSE電報機

作品名稱：摩斯電報機

介面：
1.nxt主機
2.Touch傳感器 × 1（端口1）
3。程式：NXT的LabVIEW 8.5中的工具包


功能：
1。啟動時出現ready ..字樣，等待使用者輸入
2。當有按鍵持續輸入（press and release）訊息時，每按一次（press）均會發出聲響（880Hz的音），並記錄每一次press and release pair的秒數
3。等待時間超過 2秒，開始迴聲（或恢復）你剛剛輸入的一連串訊息。
4。按橘色鍵結束程式


程式碼：

↑recall階段,False case


影片示範：




本程式參考：
echo telegraph http://www.nxtprograms.com/echo_telegraph/index.html

NXT-Rider

作品名稱：NXT-Rider
功能（或目的）：使NXT主機上的Rider，藉由軟體設計的控制器，使Rider能維持平衡而不傾倒。
控制器：可變結構滑動模式＋模糊控制＝模糊滑動模式控制

目前完成：可直立，不會後傾，但Rider仍會持續往前衝，必須靠手擋住。

程式碼：

作品：

NXT-按鈕開關計數器

一、題目： 被按下時，顏色感應燈亮起，持續按住兩秒後，蜂鳴器產生一低音響聲，清除螢幕數字為零，
之後仍持續被按住，每秒鐘產生一高音響聲，並數字累加一，
按鈕放開後，燈滅。

備註：注意按鍵過程中，突然間放開再按住，不能視為連續按住，應重新計時。

二、程式碼：

NXT 測速照相

題目：

當偵測時速超過 1 km/Hr 物體經過時(方向可能靠近也可能遠離)，顏色感應燈亮起閃兩下，也就是亮一秒滅一秒再亮一秒，表示偵測到高速物體移動，然後在螢幕上顯示速度以及方向。

程式碼：

ps.不知道為何無法點選來放大圖片？看不清楚圖檔的，請連到：

http://photo.youthwant.com.tw/mine.php?do=pic&u=1973826&bid=100121231132&pid=100208125057

實驗結果：








備註：
robolab 2.9.4C的light sensor無法做一滅一暗的控制(本身軟體的bug)，所以我改用LED燈接在portA上做測試。

這台機器人會自己研磨咖啡、煮咖啡喔！

研磨咖啡機器人

日本人做的研磨咖啡機器人，有創意，而且做得很有神韻。

使用Robolab設計-直角轉彎軌道的line follower

一、硬體：

1.NXT主機輪形機器人
2.感測器：Light sensor×2、Ultrasonic sensor×1


二、軟體：
Robolab 2.9.4C


三、軌跡地圖：

四、程式碼：

五、示範影片：

簡單的line_follower設計

一、nxt機器人要做line follow，考慮用robolab設計時，我使用了2顆nxt light sensor做為感測元件，裝置的位置如下：

2個感測元件在黑色電工膠帶的兩側白色區域。

二、程式設計原理：

1.使用task split的方式，讓sensor-2和sensor-3獨立判讀

2.當sensor-2偵測在白色區域：Motor-B 前進（forward）

　當sensor-2偵測在黑色區域：Motor-B後退（Backward）

3.當sensor-3偵測在白色區域：Motor-C 前進（forward）

　當sensor-3偵測在黑色區域：Motor-C後退（Backward）

三、本程式僅對單純的直角轉彎（沒有叉路）的應用，可有良好的表現。

四、程式碼：

nxt-自動感應燈，用robolab設計

雖然研發養成所Bridan所出的題目比賽已經結束，不過我試著用robolab去做，僅供大家參考。

題目：利用 NXT 超音波感應及顏色感應燈，設計一自動感應燈，當有人接近 30 cm 以內，讓顏色感應燈亮起，人員離開超過十秒鐘後，自動關閉感應燈。

備註：

1.因為我沒有顏色感應燈，所以我用9797所提供的燈泡接在輸出埠A。

2.一樣有計時顯示在nxt上。