Kinect技術，值得研究!

今天在大潤發特別停駐在Kinect看人打電玩的時候，不是因為電玩好玩到甚麼程度，而是Kinect感應器的技術吸引我。

-----------------
目前Kinect的技術有三大主流:Kinect一次可擷取三種東西，分別是彩色影像、3D深度影像、以及聲音訊號。

Kinect主要就是靠3D深度感應器偵測玩家的動作。中間視訊鏡頭則是用來辨識玩家身分（靠著人臉辨識和身體特徵）、以及辨識基本的臉部表情。
-----------------

這可以提供給做影像辨識的研究一個方向，傳統的影像辨識各家學派技術推陳出新，技術艱深難懂，如果能夠統一規格，當然對間接使用這項技術應用的人來說，他也許不需要再學辨識技術，只要接續這項技術的後端，繼續開發更多的應用，這真的很不錯喔。

當然，以遊戲的角度來思考，Kinect的技術優於Wii或PS，在於使用者無須背負(手持)感應器(如Wii要手持遙控器)，Kinect完全拋棄這些雜七雜八的東西，讓使用者更輕盈。

寒假再來好好研究這一項技術。

難度很高的NXT bike

NXT bike難度很高，感覺比segway更難。剛剛看youtube連結NXT_Bike(http://www.youtube.com/watch?v=VxiOy4QzD7I)，酷!但看起來就難~

機構與程式在http://www.rjmcnamara.com/2010/08/05/nxt-based-bicycle-robot/

Google App Inventor -手機藍芽遙控Segway

這兩天使用Google App Inventor 將VB版的操作平台，實現在App Inventor上，操作時可以直接在手機上使用。

Google App Inventor 設計區安排的介面如下:


操作面板上，點選藍芽連線的按鈕，選擇與之匹配的NXT，即完成連線。

程式區，結構如下:

解釋:軟體啟動後，將預備顯示的區塊設定顯示(visible true)、不想出現的先隱藏(visible false)
ListPicker.BeforePicking等待使用者回應按鈕，使用者按下後，則進入挑選清單。
清單裡頭包含已被手機偵測到的藍芽裝置都會出現在畫面中。


解釋:
當ListPicker.BeforePicking按下按鈕後，被選到的清單藍芽裝置便會與手機連線。
如果連線成功，則出現Connecting...字樣
如果連線失敗，則出現can not be connected!



解釋:
當藍芽離線後，畫面出現Wait之字樣(回到初始狀態)。



解釋:
本例僅作ForwardBtn的操作。
當使用者按下ForwardBtn按鈕，程式指定ControlMessage之變數值為F，且透過
NxtDirectCommands1.MessageWrite的方法，指定mailbox為1、message為ControlMessage，
將變數的文字傳出給NXT。(NXT為接收端，程式須設計接收訊息)

附帶一提，一個很簡單卻又很重要的一個設定，少了這個操作，你的手機還是無法與nxt連結，那就是點選NxtDirectCommands1，右邊的Properties，須指定為BluetoothClient1


最後，Connect to Device，選擇你的手機，就可以開始操作了。

-----------------
至於NXT端的程式，程式碼如下:

#include "NXCDefs.h"

#define Touch IN_1



void BTCommCheck(int conn)

{

if (BluetoothStatus(conn)!=NO_ERR)      //如果nxt未與主機連線

{

 TextOut(5,LCD_LINE2,"Error");

 Wait(1000);

 Stop(true);

}

}



//-----------------------------------------

task main()

{

　string ControlMessage;

　int power=20,power_left,power_right;

　BTCommCheck(0);

　SetSensorTouch(Touch);

　ClearScreen();

　while(SENSOR_1==0)

　{

　　　ReceiveRemoteString(MAILBOX1,true,ControlMessage);

　　　TextOut(0,LCD_LINE1,ControlMessage);

　}

}

segway-改裝後的猴子騎兩輪車

brick結構概念參考NXT Segway with Rider。

今早想採用上述的結構，但因為該組裝結構沿用我前面寫的bot balance程式，似乎會一面傾倒，懶得修改程式，畢竟修改程式調整參數，耗費的工夫相較於改裝，當然是改裝比較簡單，因此動了一下腦筋，以原來的NXT Segway with Rider的概念，以及原有的兩輪平衡車子為基礎，簡單的說就是原來的兩輪平衡車上頭，加上一隻猴子。

這一隻猴子有另一個用途，猴子身體加裝一馬達，可以調整猴子身體前傾或後仰，來改變機身的重心位置，後續的設計將朝此製作。

利用藍芽技術，讓電腦可控制NXT主機

NXT Brick本身已內建藍芽裝置，對於想透過藍芽技術對NXT予以控制的玩家早已躍躍欲試。
目前已有相當齊備的文獻:

1.有些玩家透過兩台NXT，一台當作Master、一台當作Slave，由Master主機對Slave進行直接控制。
2.透過電腦PC與NXT，由電腦端(主控端)傳送控制指令(Send Message)，並由NXT回饋數值給主控端。
3.利用Android 手機與NXT，傳送控制指令(Send Message)對NXT下達操作指令。

本文旨在建立Visual Basic 介面，利用藍芽技術，由電腦操控NXT。

一、首先，利用Visual Basic Express ，建立一個操控界面

　　　　　(圖一)

上圖，通訊埠預設COM1~4，依使用者需要自行增加，通訊埠請選擇[VB工具箱/元件/SerialPort]物件，把該物件拖曳至介面中。

二、介面中的[通訊埠]，每一個選項(COM1~4)，假設使用者選擇COM1，程式會先判斷目前電腦是否與NXT連線，如早已連線，會先中斷連線，並再次連線，以確保不重複連線的問題。

　　　　COM1ToolStripMenuItem.Checked = True

　　　　COM2ToolStripMenuItem.Checked = False

　　　　COM3ToolStripMenuItem.Checked = False

　　　　COM4ToolStripMenuItem.Checked = False

　　　　If SerialPort1.IsOpen() Then

　　　　　　SerialPort1.Close()

　　　　End If

　　　　SerialPort1.PortName = COM1ToolStripMenuItem.Text

　　　　SerialPort1.Open()

　　　　ConnText.Text = "已連線"

上述部分的程式碼，其中

1.COM1ToolStripMenuItem是介面中COM1的選項Ｎame，COM1ToolStripMenuItem.Checked用以告知使用者當前選擇哪一個通訊埠，會在該選項出現打勾
2.使用者該選擇哪一個通訊埠？請打開電腦藍芽選項，先讓電腦搜尋可與電腦藍芽連線的裝置，當然，你一定要記得先打開NXT的電源以及藍芽（ＯＮ），才能讓電腦搜尋的到。當使用者找到NXT後，會要求使用者輸入密碼，預設為1234，因此在電腦輸入1234的密碼後，便與NXT匹配連線了
3.此時，點選可與電腦連線的裝置兩下，可看到設定的選項，其中［輸出序列埠］就是預備和NXT連線的對外通訊埠。這個通訊埠即我們要選擇的。

三、介面中有[前進]、[後退]、[向左]、[向右]四個按鈕，使用者按下按鈕後，會傳出指令給NXT，因此相關設定的程式碼如下:
　　Dim byteOut(9) As Byte
　　byteOut(0) = 2 + 5 '2 bytes in output message
　　byteOut(1) = &H0 'should be 0 for NXT
　　byteOut(2) = &H80 'Direct Command no reply expected
　　byteOut(3) = &H9 'MESSAGEWRITE
　　byteOut(4) = &H0 'Box Number - 1
　　byteOut(5) = 2 + 1 '2-byte message size with null terminator
　　byteOut(6) = Asc("F")
　　byteOut(7) = Asc(" ") ' space ASCII code
　　byteOut(8) = &H0 'null terminator
　　SerialPort1.Write(byteOut, 0, 9)

1.上述程式碼是[前進]按鈕的功能。其他[後退]、[向左]、[向右]程式碼均相同，可複製貼上至其他按鈕。
2.唯一需要修改的，在於byteOut(6)。茲設定的[前進]，當按下按鈕時，送出字元F；
[後退]送出字元B；[向左]送出字元L；[向右]送出字元R
3.byteOut為電腦與NXT之間通訊的方式，相關文獻可參考[NXTreme]、[研發養成所]及[VB and Connecting the NXT with Bluetooth]
4.電腦傳輸命令至NXT，是送到指定的信箱(MAILBOX)，所以byteOut(4)=&H0是MAILBOX1，由使用者自行定義要傳到哪一個信箱，以便NXT接收。當然，NXT的程式碼，其收件信箱也要指定相同的MAILBOX，投遞與接收要相同。

至此，VB的通訊介面業已底定。
●●●●●●●●●●●●●●●●●●●●●●●●●●●●●●
其次，NXT的接收程式碼，茲以Brixcc的NXC碼編寫，程式碼如下:

#include "NXCDefs.h"



#define MAILBOX1 0

#define Touch IN_1



void BTCommCheck(int conn)

{

if (BluetoothStatus(conn)!=NO_ERR)      //如果nxt未與主機連線

{

  TextOut(5,LCD_LINE2,"Error");

  Wait(1000);

  Stop(true);

}

}



//-----------------------------------------

task main()

{

string ControlMessage;

BTCommCheck(0);

SetSensorTouch(Touch);

ClearScreen();

while(SENSOR_1==0)

{

    ReceiveRemoteString(MAILBOX1,true,ControlMessage);

    TextOut(0,LCD_LINE1,ControlMessage);

}

}

1.程式開啟後，NXT會先檢查是否與PC連線，如果未連線，則出現ERROR畫面並結束程式，使用者必須先啟動前述建立的VB介面軟體，點選通訊埠連線
2.連線後，當使用者在VB介面軟體按下[前進]、[後退]、[向左]、[向右]任一按鈕，則於NXT畫面顯示相對應的字元(F、B、L、R)
3.BTCommCheck(0)之引數0，代表NXT設定為slave端。
4.從電腦發送的命令，直至NXT接收，本身為字元，所以設定一個變數ControlMessage接收之

實現Segway 兩輪平衡車

Segway是一個非常不穩定的非線性模型，很多研究多半會選擇​segway作為鑑定其控制器設計的良窳。

Segway是一台兩輪式、自由度高、且易於往兩側傾倒的極不平​衡車子，為了維持車子能立直中立，而不易傾倒，則便是研究者致力​研究的終極目的。

現階段研究，能使機身維持筆直站立且穩定，下階段想透過手機藍芽​裝置，來遠端遙控Segway。

[執行結果]




卡丁車與segway對決

Matlab for NXT，測試光感知器每秒鐘亮滅一次設計

[說明]使用Matlab 讓光感知器每秒鐘做亮或滅的動作

[程式碼]

clear all

clc



%Define Sensor Parameters

SwitchPort = SENSOR_1;

LightPort =SENSOR_2;

Mode = 0;



COM_CloseNXT('all');



hNXT=COM_OpenNXT();

COM_SetDefaultNXT(hNXT);



OpenSwitch(SwitchPort);



while(GetSwitch(SwitchPort)==false)

　　if(Mode == 0)

　　　　ModeType = 'INACTIVE';

　　　　OpenLight(LightPort,ModeType);

　　　　pause(1);

　　end

　　if(Mode == 1)

　　　　ModeType = 'ACTIVE';

　　　　OpenLight(LightPort,ModeType);

　　　　pause(1);

　　end

　　Mode = not(Mode);

end



COM_CloseNXT(hNXT);

使用not函數，用以改變mode狀態


［執行結果］

Matlab for NXT，經卡爾門濾波前後，陀螺儀數據觀測

經卡爾門濾波前後，陀螺儀數據觀測，發現有很明顯的濾波效果，由記錄的結果可證實。

[程式碼]

% 測試陀螺儀元件- GetGyro()

clear all

clc



%IN Port 設定

touch_port = SENSOR_1;

gyroport = SENSOR_3;



%設定Kalman Filter 參數

P=10;

Q=1e-4;

R=0.05;

prev_filter_value=0;



COM_CloseNXT('all');

hNXT=COM_OpenNXT();

COM_SetDefaultNXT(hNXT);



OpenSwitch(touch_port);    %開啟觸碰感應器



OpenGyro(gyroport);        %開啟陀螺儀感知器

offset = CalibrateGyro(gyroport,'AUTO');  %計算陀螺儀偏移值(offset)



cnt=1;

while(GetSwitch(touch_port)==false)

　GyroValue= GetGyro(gyroport);        %陀螺儀原始數據

　[P,curr_filter_value]=Kalman_filter(P,Q,R,prev_filter_value,GyroValue);   %陀螺儀數據經卡爾門濾波後的數據

　disp(sprintf('GyroValue=%2.2f',curr_filter_value));

　pause(0.01);

　clc

　data(1,cnt)=GyroValue;              %紀錄原始陀螺儀數據

　data(2,cnt)=curr_filter_value;      %紀錄卡爾門濾波後的數據

　prev_filter_value=curr_filter_value;

　cnt=cnt+1;

end



CloseSensor(touch_port);   %關閉感知器

CloseSensor(gyroport);

COM_CloseNXT(hNXT);



plot(data(1,:))

hold on

plot(data(2,:),'r-')

hold off

[副程式Kalman_Filter.m]

function [P,curr_filter_value]=Kalman_filter(P,Q,R,prev_filter_value,measurement_value)

　P=P+Q;              %第k-1時刻與第k時刻前測之間的協方差P

　Kgain=P/(P+R);      %卡爾曼增益

　curr_filter_value=prev_filter_value+Kgain*(measurement_value-prev_filter_value);

　P=(1-Kgain)*P;      %第k時刻後測與前測之間的協方差

end

[執行結果]
offset=610.2152

從上圖中，藍色線代表未濾波前的數據、紅色線為濾波後的數據，前後結果非常明顯。

Matlab for NXT，陀螺儀測試偏移值

本程式碼用以測試陀螺儀(Hitechnic Gyro)之偏移值(offset)，將機器人扶正靜待1秒鐘，即可完成檢測。
程式碼如下:

% 測試陀螺儀元件

clear all

clc

% 設定陀螺儀參數

port = SENSOR_3;



COM_CloseNXT('all');

hNXT=COM_OpenNXT(); 

COM_SetDefaultNXT(hNXT);



OpenGyro(port);

offset = CalibrateGyro(port,'AUTO');



disp(sprintf('Gyro Sensor offset = %d ',offset));



COM_CloseNXT(hNXT); 

Matlab For NXT，本程式測試讀取MotorA轉角，並回傳數值至電腦

[程式碼]

clear all

clc



COM_CloseNXT('all');



hNXT=COM_OpenNXT();   %此指令會尋找使用 USB 線與電腦連接的 NXT，若是要使用藍芽的話則只要在括號內加上 'bluetooth.ini' 即可

COM_SetDefaultNXT(hNXT); %將 hNXT 設為全域的預設代碼，之後就不需要在指令後再輸入代碼了



force=50;

Dist=360;



MyMotor = NXTMotor(MOTOR_A,'Power',force, 'TachoLimit', Dist);

MyMotor.ResetPosition();



MyMotor.SendToNXT();

data=MyMotor.ReadFromNXT();



while(data.Position < Dist)    

 data=MyMotor.ReadFromNXT();    

 disp(sprintf('Current Position of Motor A: %d', data.Position));

end 



COM_CloseNXT(hNXT); 

Matlab for NXT，馬達正轉與反轉測試

將馬達接在port A,C，進行正反轉測試
[程式碼]
%測試A,C馬達正轉與反轉

clear all

clc



COM_CloseNXT('all');



hNXT=COM_OpenNXT();   %此指令會尋找使用 USB 線與電腦連接的 NXT，若是要使用藍芽的話則只要在括號內加上 'bluetooth.ini' 即可

COM_SetDefaultNXT(hNXT); %將 hNXT 設為全域的預設代碼，之後就不需要在指令後再輸入代碼了



LeftWheel=MOTOR_A;

RightWheel=MOTOR_C;

BothWheel=[LeftWheel;RightWheel];

Force=50;  %馬達動力

Dist=360;  %馬達轉動的角度



MotorForward = NXTMotor(BothWheel,'Power',Force, 'TachoLimit', Dist);

MotorReverse = MotorForward;  %設定MotorReverse結構與MotorForward相同

MotorReverse.Power= -Force;   %個別修改Power屬性



MotorForward.SendToNXT();     %送出正轉指令

MotorForward.WaitFor();       %等待執行完畢



MotorReverse.SendToNXT();     %送出反轉指令

MotorReverse.WaitFor();



COM_CloseNXT(hNXT);

RWTH- MindStorms NXT Toolbox Documents

這個網站完整記錄Matlab 編輯NXT 控制，對於感測器、馬達等元件的語法說明。
http://www.mindstorms.rwth-aachen.de/trac/wiki/Documentation

Matlab for NXT，搭配LightSensor與TouchSensor之測試實驗

以前接觸Brixcc或lejos，TouchSensor就叫做TouchSensor，但Matlab的Toolbox命名為Switch。

以程式碼解說實際的簡單應用:
[程式碼]

%觸碰感知器(TouchSensor)、光感知器(LightSensor)測試

clear all
clc

COM_CloseNXT('all');

hNXT=COM_OpenNXT();   %此指令會尋找使用 USB 線與電腦連接的 NXT，若是要使用藍芽的話則只要在括號內加上 'bluetooth.ini' 即可
COM_SetDefaultNXT(hNXT); %將 hNXT 設為全域的預設代碼，之後就不需要在指令後再輸入代碼了

OpenSwitch(Sensor_1);
OpenLight(Sensor_2,'ACTIVE');

cnt=1;
while (GetSwitch(Sensor_1)==false)
 LightValue(cnt)=GetLight(Sensor_2)*0.09775;
 cnt=cnt+1;
end

plot(LightValue)
COM_CloseNXT(hNXT);

解說:
1.這個程式利用OpenLight()指令、OpenSwitch()指令分別初始光感知器、觸碰感知器，指令的應用，請在Matlab command提示字元下，輸入help OpenLight、help OpenSwitch查閱
2.OpenLight()指令，參數使用ACTIVE，告知初始即點亮LightSensor
3.GetSwitch()用以取得觸碰感知器觸碰與否，有則回傳true，反之則以false
4.GetLight()指令用以取得光感知器的讀值，乘以0.09775轉換為我們較為了解的光感值
5.程式判斷觸碰感知器未被按下時，記錄光感知器的讀值於LightValue陣列，並於觸碰感知器被按下後，將結果plot出來。

Matlab for NXT，程式碼的外殼template

紅色文字顏色套用自程式碼中。

[程式碼]
clear all
clc
COM_CloseNXT('all');

%此指令會尋找使用 USB 線與電腦連接的 NXT，若是要使用藍芽的話則只要在括號內加上 'bluetooth.ini' 即可
hNXT=COM_OpenNXT();

%將 hNXT 設為全域的預設代碼，之後就不需要在指令後再輸入代碼了
COM_SetDefaultNXT(hNXT);

:
:
程式區
:
:



COM_CloseNXT(hNXT);

Matlab for NXT，安裝ToolBox套件

筆者過去接觸Matlab多半用於自動控制設計，對於NXT的操作算是頭一遭。如有謬誤，請指教。

環境說明:
1.使用Matlab版本為R2008b
2.作業系統OS為win7
3.NXT_Driver for win7
(1) 32bit: http://www.robotc.net/files/NXT_32bit_driver.zip
(2) 64bit: http://www.robotc.net/files/NXT_64bit_driver.zip
4. NXT Firmware: LEGO_MINDSTORMS_NXT_Firrmware_V1.29.rfw
5. RWTH Mindstorms NXT toolbox

安裝toolbox步驟說明
1.到RWTH網站下載最新toolbox

2.更新NXT firmware韌體程式至最新

3.打開matlab，點選[File/Set Path/Add Folder]，並指定[RWTHMindstormsNXT]資料夾所在位置，選擇後按[save]

4.再次選擇[Add Folder]，並指定[RWTHMindstormsNXT/tools]之資料夾加入，按[save]

5.打開RWTHMindstormsNXT 資料夾，進入子資料夾tools->MotorControl，裡面會有一個檔案MotorControl22.rxe，利用NXT-G或Brixcc將該檔案傳到NXT主機上
(所有傳呼控制都會執行此檔案，務必上傳)

6.接上NXT，進入MATLAB，在command window上輸入指令： COM_OpenNXT
如果畫面正常，將出現NXT連線後的相關資訊。