首頁新聞工控搜論壇廠商論壇產品方案廠商人才文摘下載展覽


P L C \| 變頻器與傳動 \| 傳感器 \| 現場檢測儀表 \| 工控軟件 \| 人機界麵 \| 運動控製 D C S \| 工業以太網 \| 現場總線 \| 顯示調節儀表 \| 數據采集 \| 數傳測控 \| 工業安全電源 \| 嵌入式係統 \| PC based \| 機櫃箱體殼體 \| 低壓電器 \| 機器視覺	工控搜索快.專.準

強實時運動控製內核MotionRT750(九)：內置C語言的自定義機械手模型實現
深圳市正運動技術有限公司

強實時運動控製內核MotionRT750

MotionRT750是正運動技術首家自主自研的x86架構Windows係統或Linux係統下獨占確定CPU的強實時運動控製內核。

強實時運動控製內核MotionRT750.png

該方案采用獨占確定CPU內核技術實現超強性能的強實時運動控製。它將核心的運動控製、機器人算法、數控（CNC）及機器視覺等強實時的任務，集中運行在1-2個專用CPU核上。與此同時，其餘CPU核則專注於處理Windows/Linux相關的非實時任務。

此外集成MotionRT750 Runtime實(shi)時(shi)層(ceng)與(yu)操(cao)作(zuo)係(xi)統(tong)非(fei)實(shi)時(shi)層(ceng)，並(bing)利(li)用(yong)高(gao)速(su)共(gong)享(xiang)內(nei)存(cun)進(jin)行(xing)數(shu)據(ju)交(jiao)互(hu)，顯(xian)著(zhu)提(ti)升(sheng)了(le)運(yun)動(dong)控(kong)製(zhi)與(yu)上(shang)層(ceng)應(ying)用(yong)間(jian)的(de)通(tong)信(xin)效(xiao)率(lv)及(ji)函(han)數(shu)執(zhi)行(xing)速(su)度(du)，最(zui)終(zhong)實(shi)現(xian)更(geng)穩(wen)定(ding)、更高效的智能裝備控製，確保了運動控製任務的絕對實時性與係統穩定性，特別適用於半導體、電子裝備等高速高精的應用場合。

強實時運動控製內核MotionRT750匹配產品接線.png

MotionRT750應用優勢：

1.跨平台兼容性：支持Windows/Linux係統，適配不同等級CPU。

2.開發靈活性：提供多語言編程接口，便於二次開發與功能定製。

3.實時性提升：通過CPU內核獨占機製與高效LOCAL接口，實現2-3us指令交互周期，較傳統PCI/PCIe方案提速近20倍。

4.擴展能力強化：多卡多EtherCAT通道架構支持254軸運動控製及500usEtherCAT周期。

5.係統穩定性：32軸125us EtherCAT冗餘架構消除單點故障風險，保障連續生產。

6.安全可靠性：不懼Windows係統崩潰影響，藍屏時仍可維持急停與安全停機功能有效，確保產線安全運行。

7.功能擴展性：實時內核支持C語言程序開發，方便功能拓展與實時代碼提升效率。

MotionRT750視頻介紹可點擊→正運動強實時運動控製內核MotionRT750。

更多關於MotionRT750的詳情介紹與使用點擊→強實時運動控製內核MotionRT750(一)：驅動安裝、內核配置與使用。

超實時EtherCAT運動控製卡XPCIE6032H

XPCIE6032H運動控製卡集成6路獨立EtherCAT主站接口。整卡最高可支持254軸運動控製；125usEtherCAT通訊周期時，兩個端口配置冗餘最高可支持32軸運動控製。6個EtherCAT主站各通道獨立工作，多EtherCAT主站互不影響。

超實時EtherCAT運動控製卡XPCIE6032H.png

超實時EtherCAT運動控製卡XPCIE6032H接線圖.png

XPCIE6032H視頻介紹可點擊→全球首創！PCIe 6路高性能EtherCAT運動控製卡XPCIE6032H。

XPCIE6032H運動控製卡麵向半導體設備、精密3C電子、生物醫療儀器、新能源裝備、人形機器人及激光加工等高速高精場景，為固晶機、貼片機、分選機、鋰電切疊一體機、高速異形插件設備等自動化裝備提供核心運動控製支持。

XPCIE6032H硬件特性：

1.EtherCAT通訊周期可到125us（需要主機性能與實時性足夠)。

2.板卡集成6路獨立的EtherCAT主站接口，最多可支持254軸運動控製。

3.搭載運動控製實時內核MotionRT750。

4.相較於傳統的PCI/PCIe、網口等通訊方式，速度可提升了10-100倍以上。

5.板載16路高速輸入，16路高速輸出。

6.板載4路高速鎖存，4路通用PWM輸出。

更多關於XPCIE6032H的詳情介紹與使用點擊→全球首創！PCIe超實時6通道EtherCAT運動控製卡上市！。

超實時EtherCAT運動控製卡XPCIE2032H

XPCIE2032H集成2路獨立EtherCAT接口。整卡最高可支持至254軸運動控製；125usEtherCAT通訊周期時，單接口最高可支持32軸運動控製。2個EtherCAT主站各通道獨立工作，多EtherCAT主站互不影響。

超實時EtherCAT運動控製卡XPCIE2032H.png

雙EtherCAT主站端口可任意設置為以下通道，且兩個端口也設置為不同類型通道:

● 高速通道 - EtherCAT通訊周期125us

● 常規通道 - EtherCAT通訊周期250us-8ms

超實時EtherCAT運動控製卡XPCIE2032H槽位示意.png

超實時EtherCAT運動控製卡XPCIE2032H接線圖.png

XPCIE2032H視頻介紹可點擊→高速高精運動控製！PCIe超實時2通道EtherCAT運動控製卡上市！。

XPCIE2032H硬件特性：

1.EtherCAT通訊周期可到125us（需要主機性能與實時性足夠)。

2.板卡集成2路獨立的EtherCAT主站接口，最多可支持254軸運動控製。

3.搭載運動控製實時內核MotionRT750。

4.相較於傳統的PCI/PCIe、網口等通訊方式，速度可提升了10-100倍以上。

5.板載8路高速輸入，16路高速輸出。

6.板載4路高速鎖存，4路通用PWM輸出。

更多關於XPCIE2032H的詳情介紹與使用點擊→高速高精運動控製！PCIe超實時2通道EtherCAT運動控製卡上市！。

超實時EtherCAT運動控製卡XPCIE1032H

XPCIE1032H是一款基於PCI Express的EtherCAT總線運動控製卡，可選6-64軸運動控製，支持多路高速數字輸入輸出，可輕鬆實現多軸同步控製和高速數據傳輸。

超實時EtherCAT運動控製卡XPCIE1032H.png

超實時EtherCAT運動控製卡XPCIE1032H接線圖.png

XPCIE1032H視頻介紹可點擊→高性能PCIe EtherCAT運動控製卡 | XPCIE1032H_。

XPCIE1032H運動控製卡集成了強大的運動控製功能，結合MotionRT7運動控製實時軟核，解決了高速高精應用中，PC Windows開發的非實時痛點，指令交互速度比傳統的PCI/PCIe快10倍。

XPCIE1032H硬件特性：

1.6-64軸EtherCAT總線+脈衝可選，其中4路單端500KHz脈衝輸出。

2.16軸EtherCAT同步周期500us，支持多卡聯動。

3.板載16點通用輸入，16點通用輸出，其中8路高速輸入和16路高速輸出。

4.通過EtherCAT總線，可擴展到512個隔離輸入或輸出口。

5.支持PWM輸出、精準輸出、PSO硬件位置比較輸出、視覺飛拍等。

6.支持直線插補、圓弧插補、連續軌跡加工（速度前瞻）。

7.支持電子凸輪、電子齒輪、位置鎖存、同步跟隨、虛擬軸、螺距補償等功能。

8.支持30+機械手模型正逆解模型算法，比如SCARA、Delta、UVW、4軸/5軸 RTCP...

更多關於XPCIE1032H詳情點擊“不止10倍提速！PCIe EtherCAT實時運動控製卡XPCIE1032H 等您評測！”查看。

如何使用C語言與BASIC語言進行配合

C語言與BASIC語言配合.png

基本使用方法

01 固件版本要求

控製器使用C函數需要使用支持C接口函數的固件版本；固件版本名稱裏帶有“cfunc”的即為支持C函數接口。

02 函數調用限製

同一個C文件內的某一個C函數隻能在某一個Basic文件內被聲明調用，不可被在多個basic文件內都進行聲明調用。

03 多函數聲明規則

同一個C文件內的不同C函數可以分別在不同的Basic文件內被聲明調用，但再次被聲明後的函數名不可一致。

DEFINE_CFUNC -- 關鍵字

DEFINE_CFUNC關鍵字.png

支持的數據類型定義：int、float、double、TYPE_TABLE。如果與TABLE數組交互，建議使用TYPE_TABLE類型。在4係列以上的控製器，TYPE_TABLE是double類型。

舉例一

C語言編程部分:

int userc_init(void)
{
    int* p=(int *)malloc(sizeof(int));
    p[0]=88;
    printf("p[0]=%d\n",p[0]);
    free(p);
    return 0;
}
float divf(float a,float b)
{
    return (a/b);
}
TYPE_TABLE divd(TYPE_TABLE a,TYPE_TABLE b)
{
    return (a/b);
}

BASIC編程部分:

define_cfunc userc_init int userc_init(void)
define_cfunc userc_divf float divf(float a,float b)
define_cfunc userc_divd double divd(double a,double b)
?userc_init()
?userc_divf(23.1,1)
?userc_divd(23.3,1)

舉例一.png

以下程序代碼測試TABLE指針輸入輸出數據。

舉例二

C語言編程部分:

int tablefunc(TYPE_TABLE *ptable,int inum)
{
    int i;
    for(i=0;i

 

BASIC編程部分:

define_cfunc userc_tfunc int tablefunc(TYPE_TABLE *ptable,int inum)
dim ptable(20)
dim inum
userc_tfunc(ptable,inum)



注意事項：

（1）定義的無參數函數，可以在INT_CYCLE中直接使用。

（2）BASIC調用的C函數的參數個數最多支持8個。

（3）C函數注意安全性，注意代碼規範性，否則可能導致死機。

（4）C函數要注意實時性，處理必須夠快，否則會影響BASIC的實時性。

注：建議調試時都下載RAM運行！

 

 

 

編譯平台選擇參照

不同型號控製器編譯平台有所不同，具體參照下表。目前僅以下型號控製器支持C語言，其他係列型號控製器如有疑問請與技術工程師聯係。



右鍵單擊“文件視圖”中空白區域，點擊彈出窗口中“設置”一欄，進行編譯平台設置操作。在彈出窗口“編譯平台”一欄中點擊下拉列表，在下拉列表中選擇相應的編譯平台後單擊“確定”，即可完成編譯平台設置操作。



 

C函數使用步驟

1.在RTSys軟件中單擊菜單欄“文件”，在下拉窗口中選擇“新建項目”。選擇項目文件存入的路徑並且自定義命名項目名稱。



2.新建項目成功後，在新建的項目下新建新的Basic文件，並且自定義相關Basic文件名。點擊確定Basic文件創建成功。



3.新建.C文件。步驟可參照第二步，新建文件類型時需選擇“C”。



4.右鍵單擊“文件視圖”中空白區域，點擊彈出窗口中“設置”一欄，進行編譯平台設置操作。



5.在彈出窗口“編譯平台”一欄中點擊下拉列表，在下拉列表中選擇相應的編譯平台後單擊“確定”，即可完成編譯平台設置操作，編譯平台的選擇可參照上一章節“編譯平台選擇參考”。



6.編寫C函數。編寫C程序時需先進行頭文件聲明步驟，然後再按照C語言編碼規範編寫C函數。

頭文件聲明語法：#include “xxxxx.h”。“xxxx.h”為引用內置函數的頭文件。如下圖所示，“.h”頭文件的文件路徑需與項目文件路徑一致。“zmcbuildin.h”文件可聯係正運動工程師獲取。



7.在BASIC程序中使用C函數時需要使用關鍵字“DEFINE_CFUNC”對被使用C函數進行引用定義，定義後在Basic文件中使用重新賦予的函數名即可調用該C函數。



注：C函數代碼在RTSys中直接編輯即可，在其他軟件中編輯複製到RTSys中時可能會導致亂碼或格式錯誤等問題。

 

簡單示例

1.聲明C函數接口（zmcbuildin.h）

C語言：

// 添加自定義函數聲明
#include "zmcbuildin.h"
double fast_sin() 
{
    double sum = 0;
    for(int i=0; i<100000; i++){
        sum = sum + sin(i*0.01);
    }
    return sum;
}

2.BASIC綁定CFUNC

Basic語言：

DEFINE_CFUNC c_sin_calc double fat_sin()
```

3.混合編程調用

Basic語言：

sum = c_sin_calc()



 

 

 

自定義機械手模型實現

已有的機械手類型如下，更多類型可以查看《正運動機械手手冊》。









 

對於需要實現自定義機械手模型的需求，我們內置C語言提供了CFRAME機械手結構擴展功能，用戶可通過此功能自定義機械手模型，C函數部分使用 usercframe.c文件內的即可，進行算法自定義。函數用法示例相關代碼介紹如下：

 

 

C函數部分如下：

正運動控製器內置Frame1~Frame999的機械手類型，用戶自定義機械手類型為FRAME1000~FRAME65535。

函數名自動根據frame編號生成，下麵C語言部分根據1000來生成舉例。

功能：擴展機械手frame類型



 

功能：frame初始化

//機械手每次正逆解回調執行，用戶可以初始化內部變量
// 輸入:
//        pzmc	           控製器描述結構指針
//        pframe	       機械手基本狀態指針
//        pParaList Table  參數列表
// 返回值, 成功與否, 0-OK
********************************/
uint32 SOFRAME_INIT1000(struct_soZmcDisp *pzmc,  struct_soFrameStatus* pframe, TYPE_TABLE* pParaList)
{
    //初始化
    int16 i;
     //把connreframe的table數值賦值給臂長 和關節脈衝
    g_soframeinfo[0].m_flen1 = *pParaList;
    g_soframeinfo[0].m_flen2 = *(pParaList + 1);
    g_soframeinfo[0].m_flen3 = *(pParaList + 2);
    g_soframeinfo[0].m_flen4 = *(pParaList + 3);
    //關節一圈脈衝
    g_soframeinfo[0].m_pulse1 =  1000;  //為了安全起見 demo故意設置 1
    g_soframeinfo[0].m_pulse2 =  1000;  //*(pParaList + 5);
    g_soframeinfo[0].m_pulse3 =  1000;
    g_soframeinfo[0].m_pulse4 =  1000;
    //...
    g_soframeinfo[0].m_pulsev = 1000;
    for(i=0;im_pPrivate = (void *)&g_soframeinfo[0];
    //更新當前機械手姿態
    pframe->m_iHand = 0;//具體用戶自己編寫計算機械手當前實際姿態，姿態數值含義用戶自己定義，
    pf = (struct_userframeinfo *)pframe->m_pPrivate;
    if(NULL == pf)
    {
        return -1;
    }
    //打印輸出測試
     rtprintf("init %.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%d\n",pf->m_flen1,pf->m_flen2,pf->m_flen3,pf->m_flen4,pf->m_pulse1,pf->m_pulse2,pf->m_pulse3,pf->m_pulse4,pf->m_pulsev,pframe->m_iHand);
    return 0;
}

 

 

功能：frame正解

//關節坐標轉換世界坐標
//計算焊槍末端坐標
// 輸入:
//        pzmc	  控製器描述結構指針
//        pframe	  機械手基本狀態指針
//        pfJointPulsein	輸入關節脈衝坐標
// 輸出
//        pHand	    輸出當前姿態
//        pfWorldout輸出units單位世界坐標WPOS
// 返回值, 成功與否, 0-OK 
********************************/
uint32 SOFRAME_TRANS1000(struct_soZmcDisp *pzmc,  struct_soFrameStatus* pframe, TYPE_FRAME*pfJointPulsein, int32 *pHand, TYPE_FRAME* pfWorldout)
{
    int i;
    double uj[6],pout[6];
    struct_userframeinfo  *pf =NULL;
    pf = (struct_userframeinfo *)pframe->m_pPrivate;
    if(NULL == pf)
    {
        return -1;
    }
    //把輸入脈衝轉為角度
    uj[0] = *(pfJointPulsein + 0) / pf->m_pulse1 ;  //關節一 實時橫焊位置  度
    uj[1] = *(pfJointPulsein + 1) / pf->m_pulse2 ;  //關節二 實時高度位置  度
    uj[2] = *(pfJointPulsein + 2) / pf->m_pulse3 ;  //關節三 實時鍾擺角度  度
    uj[3] = *(pfJointPulsein + 3) / pf->m_pulse4 ;  //關節四 實時俯仰角度  度
    //轉弧度
    uj[0] = uj[0] / 180 * PI;
    uj[1] = uj[1] / 180 * PI;
    uj[2] = uj[2] / 180 * PI;
    uj[3] = uj[3] / 180 * PI;
    //正解過程......
    //計算姿態，返回姿態
    *pHand = 0;
    //demo 計算出世界坐標
    pout[0] = 100; //x   mm
    pout[1] = 100; //y   mm
    pout[2] = 100; //z   mm
    pout[3] = 90; //Rz  角度
    pfWorldout[0] = pout[0] ;
    pfWorldout[1] = pout[1] ;
    pfWorldout[2] = pout[2] ;
    pfWorldout[3] = pout[3] ;
    //打印輸出測試
     if(0 == g_printflag)
    {
        rtprintf("trans input %.2f,%.2f,%.2f,%.2f, output ,%.2f,%.2f,%.2f,%.2f\n",uj[0],uj[1],uj[2],uj[3],pfWorldout[0],pfWorldout[1],pfWorldout[2],pfWorldout[3]);
        g_printflag = 1;
    }
    return 0;
}

 

 

功能：frame逆解

//世界坐標轉換關節坐標
// 輸入:
//      pzmc	  控製器描述結構指針
//      pframe	  機械手基本狀態指針
//      pfWorldin	輸入世界坐標units單位
//      ihand	  輸入坐標對應姿態, -1表示使用當前姿態.
// 輸出：
//      pfJointPulseout	輸出關節脈衝坐標
// 返回值, 成功與否, 0-OK 
********************************/
uint32 SOFRAME_RETRANS1000(struct_soZmcDisp *pzmc,  struct_soFrameStatus* pframe, TYPE_FRAME *pfWorldin, int32 ihand, TYPE_FRAME* pfJointPulseout)
{
    int i;
    double uw[6],pout[6];
    struct_userframeinfo  *pf = NULL;
    pf = (struct_userframeinfo *)pframe->m_pPrivate;
    if(NULL == pf)
    {
        return -1;
    }
    //把世界坐標脈衝轉為mm和角度  pfWorldin 已經是除以units的值了
    uw[0] = pfWorldin[0]  ;   // 位置 x   mm
    uw[1] = pfWorldin[1]  ;   // 位置 y   mm
    uw[2] = pfWorldin[2]  ;   // 位置 z   mm
    uw[3] = pfWorldin[3]  ;   // 位置 roll 弧度
     //轉弧度
    uw[3] = uw[3] / 180 *PI;
     //逆解過程......
    //計算出關節坐標
    pout[0] = uw[0]; //關節1   角度
    pout[1] = 60; //關節2   角度
    pout[2] = 60; //關節3   角度
    pout[3] = 30; //關節4   角度
    //demo 為了安全起見 不改變關節輸出
    pfJointPulseout[0] = pout[0] * 1000;
    pfJointPulseout[1] = pout[1] * 1000;
    pfJointPulseout[2] = pout[2] * 1000;
    pfJointPulseout[3] = pout[3] * 1000;
    //打印輸出測試
    if(1 == g_printflag)
    {
        rtprintf("retrans input %.2f,%.2f,%.2f,%.2f, output ,%.2f,%.2f,%.2f,%.2f\n",uw[0],uw[1],uw[2],uw[3],pfJointPulseout[0],pfJointPulseout[1],pfJointPulseout[2],pfJointPulseout[3]);
        g_printflag = 0;
     }
    return 0;
}

 

 

功能：自定義坐標旋轉函數DPOS轉換為WPOS

// 輸入:
//      pzmc	  控製器描述結構指針
//      pframe	  機械手基本狀態指針
//      pfRotate  坐標平移旋轉的參數, 依次為XYZ,RX,RY,RZ
//      pfin 	  輸入虛擬軸坐標列表, units單位
// 輸出
//      pfout 	  輸出世界坐標列表, units單位
// 返回值, 成功與否, 0-OK
********************************/
uint32 SOFRAME_ROTATETOWPOS1000(struct_soZmcDisp *pzmc,  struct_soFrameStatus* pframe, TYPE_FRAME *pfRotate, TYPE_FRAME *pfin, TYPE_FRAME* pfout)
{
    int i;
    rtprintf("DPOS轉換為WPOS: %0.4f, %0.4f, %0.4f\n",*(pfRotate),*(pfRotate + 1),*(pfRotate + 3));
    // 輸入的就是世界坐標係, 轉units坐標係
    for(i=0; i< pframe->m_iTotalAxisesNoAux; i++)
    {
        pfout[i] = pfin[i];
    }
    rtprintf("pfin %0.4f, %0.4f, %0.4f\n",*(pfin),*(pfin + 1),*(pfin + 3));
    rtprintf("pfout %0.4f, %0.4f, %0.4f\n",*(pfout),*(pfout + 1),*(pfout + 3));
    return 0;
}

 

 

功能：自定義坐標旋轉函數WPOS轉換為DPOS

// 輸入:
//      pzmc	  控製器描述結構指針
//      pframe	  機械手基本狀態指針
//      pfRotate  坐標旋轉的參數
//      pfin 	  輸入世界坐標列表, units單位
// 輸出
//      pfout 	  輸出虛擬軸坐標列表, units單位
// 返回值, 成功與否, 0-OK
uint32 SOFRAME_ROTATETODPOS1000(struct_soZmcDisp *pzmc,  struct_soFrameStatus* pframe, TYPE_FRAME *pfRotate, TYPE_FRAME *pfin, TYPE_FRAME* pfout)
{
    int i;
    rtprintf("WPOS轉換為DPOS: %0.4f, %0.4f, %0.4f\n",*(pfRotate),*(pfRotate + 1),*(pfRotate + 3));
    // 輸入的就是世界坐標係, 轉units坐標係
    for(i=0; i< pframe->m_iTotalAxisesNoAux; i++)
    {
        pfout[i] = pfin[i];
    }
    rtprintf("pfin %0.4f, %0.4f, %0.4f\n",*(pfin),*(pfin + 1),*(pfin + 3));
    rtprintf("pfout %0.4f, %0.4f, %0.4f\n",*(pfout),*(pfout + 1),*(pfout + 3));
    return 0;
}

 

 

 

Basic函數部分如下：

'''''電機、機械手參數定義 
dim L1                                
dim L2                            
dim L3              
dim L4              
dim PulesVROneCircle   '虛擬姿態軸一圈脈衝數 
'函數接口聲明  
DEFINE_CFRAME  1000,4,1,0,1000    'framenum, totalaxises, axises_aux,  max_attitudes,  rotatetype
'機械手參數設置 
L1                =19.8736        '1軸到2軸的X偏移
L2                =455.1143       '大擺臂長度
L3                =39.8611        '3軸中心到4軸中心距離
L4                =430.9516       '4軸到5軸的距離。 
PulesVROneCircle=360*1000
dim u_j1                '關節1實際一圈脈衝數
dim u_j2                '關節2實際一圈脈衝數
dim u_j3                '關節3實際一圈脈衝數
dim u_j4                '關節4實際一圈脈衝數 
u_j1=360*1000            '關節1實際一圈脈衝數
u_j2=360*1000                '關節2實際一圈脈衝數
u_j3=360*1000                '關節3實際一圈脈衝數
u_j4=360*1000                '關節4實際一圈脈衝數 
'''''關節軸設置
BASE(0,1,2,3)                             '選擇關節軸號0、1、2、3
atype=0,0,0,0                  '軸類型設為脈衝軸
UNITS = u_j1/360,u_j2/360,u_j3/360,u_j4/360  '把units設成每°脈衝數
DPOS=0,0,0,0                          '設置關節軸的位置，此處要根據實際情況來修改。
speed=100,100,100,100                        '速度參數設置
accel=1000,1000,1000,1000
decel=1000,1000,1000,1000
'''''''''''''''''''''虛擬軸設置'''''''''''''''''''''
BASE(6,7,8,9)
ATYPE=0,0,0,0                                            '設置為虛擬軸
TABLE(0,L1,L2,L3,L4,u_j1,u_j2,u_j3,u_j4,PulesVROneCircle)        '根據手冊說明填寫參數
speed=100,100,100,100                        '速度參數設置
UNITS=1000,1000,1000,1000       '運動精度，要提前設置，中途不能變化
'''''''''''''''''''''建立機械手連接'''''''''''''''''''''
while 1
    if scan_event(in(0))0  then        '輸入0下降沿觸發
        BASE(6,7,8,3)                                 '選擇虛擬軸號
        CONNREFRAME(1000,0,0,1,2,3)                 '啟動正解連接。
        WAIT LOADED                                 '等待運動加載。
        ?"正解模式"
    endif
wend



運行測試效果

該例子參數均為模擬，主要目的為介紹各函數接口使用方法。通過檢測輸入口0的狀態進行正逆解切換：輸入口0上升沿觸發時進入正解模式，輸入口0下降沿觸發時進入逆解模式。實際使用時根據機械手參數進行填寫。

機械手每次正逆解回調執行都會執行frame初始化函數，從參數表中讀取機械手的臂長參數，設置關節脈衝參數(示例中設為固定值1000），保存參數到全局結構體並關聯到機械手狀態，設置初始機械手姿態為0，打印初始化參數用於調試。

進入正解模式：通過函數SOFRAME_TRANS1000，將輸入的關節脈衝值轉換為角度值，將角度值轉換為弧度值，執行正解運動學計算(示例中除關節軸0其他直接返回固定值)，設置輸出姿態，將計算結果轉換為世界坐標係輸出，使用打印標誌控製調試信息的輸出頻率。

進入逆解模式：通過函數SOFRAME_RETRANS1000，將輸入的世界坐標轉換為適當單位，執行逆解運動學計算(示例中直接返回固定值)，將計算結果轉換為關節脈衝值輸出，使用打印標誌控製調試信息的輸出頻率。

 

注意事項：

1.函數命名必須遵循約定：SOFRAME_前綴+功能名+Frame編號。

2.確保正確處理單位轉換(脈衝↔單位)。

3.逆解函數需要處理多種姿態情況(ihand參數)。

4.自定義旋轉功能僅在rotatetype≥100時需要實現。

5.所有函數返回0表示成功，非0表示錯誤。

通過以上步驟，可以成功實現自定義機械手的CFRAME擴展功能。

需要更詳細的技術交流，請聯係正運動技術。

 

教學視頻可點擊→強實時運動控製內核MotionRT750(九)：內置C語言的自定義機械手模型實現查看。

 



正運動技術專注於運動控製技術研究和通用運動控製軟硬件產品的研發，是國家級高新技術企業。正運動技術彙集了來自華為、中(zhong)興(xing)等(deng)公(gong)司(si)的(de)優(you)秀(xiu)人(ren)才(cai)，在(zai)堅(jian)持(chi)自(zi)主(zhu)創(chuang)新(xin)的(de)同(tong)時(shi)，積(ji)極(ji)聯(lian)合(he)各(ge)大(da)高(gao)校(xiao)協(xie)同(tong)運(yun)動(dong)控(kong)製(zhi)基(ji)礎(chu)技(ji)術(shu)的(de)研(yan)究(jiu)，是(shi)國(guo)內(nei)工(gong)控(kong)領(ling)域(yu)發(fa)展(zhan)最(zui)快(kuai)的(de)企(qi)業(ye)之(zhi)一(yi)，也(ye)是(shi)國(guo)內(nei)少(shao)有(you)、完整掌握運動控製核心技術和實時工控軟件平台技術的企業。主要業務有：運動控製卡_運動控製器_EtherCAT運動控製卡_EtherCAT控製器_運動控製係統_視覺控製器__運動控製PLC_運動控製_機器人控製器_視覺定位_XPCIe/XPCI係列運動控製卡等等。

狀　態：離線

公司簡介
 產品目錄

公司名稱：	深圳市正運動技術有限公司
聯係人：	戴德弟
電　　話：	0755-32976042
傳　　真：	0755-2606 6955
地　　址：	深圳市寶安區西鄉洲石路陽光工業園A1棟5樓
郵　　編：	518100
主　　頁：	http://www.zmotion.com.cn