1、MCCL 在Robot上之應(yīng)用,陳文泉 2011.12.06,1,MCCL 在Robot上之應(yīng)用說明,運(yùn)動控制函式進(jìn)階應(yīng)用 Custom Motion Robot Library Dynamic Blend Robot ITE 的使用說明(Demo),2,3,MCCL 架構(gòu),MCCL,IPO,PCL,定位控制核心；因定位控制目前由硬體實(shí)作，故此模組最重要的功能轉(zhuǎn)為與硬體溝通,運(yùn)動路徑規(guī)劃核心；掌管所有運(yùn)動之路徑規(guī)劃(回Home除外)，為MCCL中最重要的模組,使用者介面；提供函式化介面供使用者更方便達(dá)到運(yùn)動控制的目的,外掛 模組,外掛模組；負(fù)責(zé)直角座標(biāo)及關(guān)節(jié)座標(biāo)間之轉(zhuǎn)換及擴(kuò)充函式功能,4,運(yùn)動

2、控制函式庫操作特性,Command Queue(10000 Spaces),MCC_Line(),MCC_PtP(),MCC_Arc(),Space is available,Put,Get,Interpolation,Software(MCCL),Hardware (Motion Control Card),FMC FIFO,Send Pulses,非直角座標(biāo)系運(yùn)動,MCCL 需使用在直角座標(biāo)系系統(tǒng) 當(dāng)使用在非直角座標(biāo)系系統(tǒng)使用者需自行進(jìn)行座標(biāo)轉(zhuǎn)換工作,5,A,B,C,X,Z,Y,(X, Y, Z) = F(A, B, Z),6,直角座標(biāo) vs 關(guān)節(jié)座標(biāo),座標(biāo)位置 (Position),關(guān)節(jié)

3、座標(biāo)位置 (Joint position),卡氐座標(biāo)位置 (Cartesian position),座標(biāo)轉(zhuǎn)換,InK,FwK,7,使用標(biāo)準(zhǔn)運(yùn)動控制函式庫- 將一筆命令分解成多個小線段,直線運(yùn)動命令,圓周運(yùn)動命令,單一小線段的運(yùn)動距離,8,Linear Motion,MCC_Line_V6( 10, 20, 30, 0, 0.5, ),座標(biāo)位置 (Position),關(guān)節(jié)座標(biāo)位置 (Joint position) () () () () (),卡氐座標(biāo)位置 (Cartesian position) ( 2, 4, 6, 0, 0.1, 0.2) ( 4, 8, 12, 0, 0.2, 0.4)

4、( 6, 12, 18, 0, 0.3, 0.6) ( 8, 16, 24, 0, 0.4, 0.8) (10, 20, 30, 0, 0.5, 1.0),9,Point-to-Point Motion,MCC_PtP_V6( 0.5, 1, 1.5, 2, 2.5, 3 ),座標(biāo)位置 (Position),關(guān)節(jié)座標(biāo)位置 (Joint position) (0.1, 0.2, 0.3, 0.4, 0.5, 0.6) (0.2, 0.4, 0.6, 0.8, 1.0, 1.2) (0.3, 0.6, 0.9, 1.2, 1.5, 1.8) (0.4, 0.8, 1.2, 1.6, 2.0, 2.

5、4) (0.5, 1.0, 1.5, 2.0, 2.5, 3.0),卡氐座標(biāo)位置 (Cartesian position) () () () () (),多個小線段的時間與速度,造成運(yùn)動不連續(xù) 速度無法提高,10,11,直角座標(biāo) vs 關(guān)節(jié)座標(biāo),12,x,y,z=280,p1= (296,74),p2= (222,148),p3= (148,222),p4= (74,296),p0= (370,0),pf= (0, 370),Keep w = 0 0 -1T for all positions,Determine the intermediate points (end-effector sp

6、ace),13,使用運(yùn)動控制函式庫擴(kuò)充模組- 將一筆小線段縮短到每個插值點(diǎn),直線運(yùn)動命令,圓周運(yùn)動命令,Interpolation Time,14,Determine the intermediate points,15,MCCL 函式庫擴(kuò)充模組架構(gòu),MCCL,IPO,PCL,定位控制核心；因定位控制目前由硬體實(shí)作，故此模組最重要的功能轉(zhuǎn)為與硬體溝通,運(yùn)動路徑規(guī)劃核心；掌管所有運(yùn)動之路徑規(guī)劃(回Home除外)，為MCCL中最重要的模組,使用者介面；提供函式化介面供使用者更方便達(dá)到運(yùn)動控制的目的,外掛 模組,外掛模組；負(fù)責(zé)直角座標(biāo)及關(guān)節(jié)座標(biāo)間之轉(zhuǎn)換及擴(kuò)充函式功能,MCCLPCI_50.DLL,R

7、obot.DLL,16,Forward,Inverse,運(yùn)動控制函式庫擴(kuò)充函式(一),外掛模組,Extension Memory,USER,Call MCC_SetKinematicTrans(TRUE),if PTP,Y,N,17,擴(kuò)充模組包含那些功能,Robot 機(jī)構(gòu)參數(shù) rbt_SetDHParam, rbt_GetDHParam 正向運(yùn)動學(xué) VS6556E_fwk/GR985_fwk 反向運(yùn)動學(xué) VS6556E_ink/GS985_ink 客製化函式 rbt_SetKinematicTrans rbt_Line_V6,機(jī)械臂機(jī)構(gòu)參數(shù)設(shè)定,18,a1,a2,a3,d4,d6,機(jī)械臂機(jī)構(gòu)參

8、數(shù)設(shè)定,19,使用 rbt_SetDHParam(DH_Param p) 讀取參數(shù)設(shè)定 使用 rbt_GetDHParam(DH_Param double a2; double a3; double d4; double d6; double origin_offset_z; DH_PARAM;,VS-6556E,正向運(yùn)動學(xué)CallBack 函式,20,CallBack 函式進(jìn)入點(diǎn),逆向運(yùn)動學(xué)CallBack 函式,21,CallBack 函式進(jìn)入點(diǎn),運(yùn)動控制函式庫擴(kuò)充函式,Supported 2 types of DH form kinematics Building SCARA type

9、kinematics,22,使用MCCL m_pwndParameters-GetDHParam( MCC_SetSysMaxSpeed( profile.dfMaxCSpeed ) MCC_SetMacParam( MCC_LineX(10); MCC_SetFeedSpeed(20); MCC_LineX(20); MCC_EnableBlend(); MCC_SetFeedSpeed(10); MCC_LineX(10); MCC_SetFeedSpeed(20); MCC_LineX(20);,當(dāng)啟動平滑運(yùn)動功能後, 必須完成全部的運(yùn) 動命令後, MCC_GetMotionStatus

10、() 的傳回 值才為 1,立即性平滑運(yùn)動(Dynamic Blend),52,立即性平滑運(yùn)動(Dynamic Blend),53,立即性平滑運(yùn)動(Dynamic Blend),使用MCC_EnableBlendInstant() Command Queue 中只能有一筆命令 當(dāng)有新的運(yùn)動命令進(jìn)來 終點(diǎn)目標(biāo)會立即被修正, 並以平滑運(yùn)動轉(zhuǎn)向,54,立即平滑運(yùn)動功能(continued),55,MCC_EnableBlend(); MCC_SetFeedSpeed(10); MCC_LineX(10); MCC_SetFeedSpeed(20); MCC_LineX(20); MCC_EnableB

11、lendInstant(); MCC_SetFeedSpeed(10); MCC_LineX(10); Wait until X 5 MCC_SetFeedSpeed(20); MCC_LineX(20);,Robot ITE 功能說明(模式選擇),56,參數(shù)設(shè)定(Parameters)、 列點(diǎn)模式(Location List)、 程式模式(Program) 、 顯示模式(Scope) 、 教導(dǎo)模式(Teach Wizard) 3-D 模擬(RoboWorks),模式選擇視窗,Robot ITE 功能說明(參數(shù)設(shè)定),57,可以讓使用者重新定義D-H參數(shù)、機(jī)構(gòu)參數(shù)及Motion Profile

12、 機(jī)構(gòu)參數(shù)包括反轉(zhuǎn)、 轉(zhuǎn)速(RPM)、PPR、 轉(zhuǎn)速比(Gear ratio)、 最大及最小轉(zhuǎn)動極限。 Motion Profile包括 最大角加速度、 最大速度及加速度,Robot ITE 功能說明(訊息顯示),58,訊息顯示視窗,顯示內(nèi)容包含Joint座標(biāo)(J1J6)與Cartesian座標(biāo)。 Cartesian座標(biāo)顯示X、Y、Z三軸的位移， 指向的Raw- Pitch- Yaw角度Rx、Ry、Rz 姿態(tài)參數(shù)Arm、Elbow、Wrist、J4及J6,Robot ITE 功能說明,RobotITE with 3D Simulator Demo,59,Discussion floattagValues6 = 0.0f; charfilename = 6DOFRobot; / this is not case-sensitive #include Simulationrobotalk.h charipAddress = SELF; /127.0.0.1 Connect(filename, ipAddress) = CONNECT_FAILED SetT