強(qiáng)實(shí)時(shí)運(yùn)動(dòng)控制內(nèi)核MotionRT750

MotionRT750是正運(yùn)動(dòng)技術(shù)首家自主自研的x86架構(gòu)Windows系統(tǒng)或Linux系統(tǒng)下獨(dú)占確定CPU的強(qiáng)實(shí)時(shí)運(yùn)動(dòng)控制內(nèi)核。

該方案采用獨(dú)占確定CPU內(nèi)核技術(shù)實(shí)現(xiàn)超強(qiáng)性能的強(qiáng)實(shí)時(shí)運(yùn)動(dòng)控制。它將核心的運(yùn)動(dòng)控制、機(jī)器人算法、數(shù)控（CNC）及機(jī)器視覺(jué)等強(qiáng)實(shí)時(shí)的任務(wù)，集中運(yùn)行在1-2個(gè)專(zhuān)用CPU核上。與此同時(shí)，其余CPU核則專(zhuān)注于處理Windows/Linux相關(guān)的非實(shí)時(shí)任務(wù)。

此外集成MotionRT750 Runtime實(shí)時(shí)層與操作系統(tǒng)非實(shí)時(shí)層，并利用高速共享內(nèi)存進(jìn)行數(shù)據(jù)交互，顯著提升了運(yùn)動(dòng)控制與上層應(yīng)用間的通信效率及函數(shù)執(zhí)行速度，最終實(shí)現(xiàn)更穩(wěn)定、更高效的智能裝備控制，確保了運(yùn)動(dòng)控制任務(wù)的絕對(duì)實(shí)時(shí)性與系統(tǒng)穩(wěn)定性，特別適用于半導(dǎo)體、電子裝備等高速高精的應(yīng)用場(chǎng)合。

MotionRT750應(yīng)用優(yōu)勢(shì)：

1.跨平臺(tái)兼容性：支持Windows/Linux系統(tǒng)，適配不同等級(jí)CPU。

2.開(kāi)發(fā)靈活性：提供多語(yǔ)言編程接口，便于二次開(kāi)發(fā)與功能定制。

3.實(shí)時(shí)性提升：通過(guò)CPU內(nèi)核獨(dú)占機(jī)制與高效LOCAL接口，實(shí)現(xiàn)2-3us指令交互周期，較傳統(tǒng)PCI/PCIe方案提速近20倍。

4.擴(kuò)展能力強(qiáng)化：多卡多EtherCAT通道架構(gòu)支持254軸運(yùn)動(dòng)控制及500usEtherCAT周期。

5.系統(tǒng)穩(wěn)定性：32軸125us EtherCAT冗余架構(gòu)消除單點(diǎn)故障風(fēng)險(xiǎn)，保障連續(xù)生產(chǎn)。

6.安全可靠性：不懼Windows系統(tǒng)崩潰影響，藍(lán)屏?xí)r仍可維持急停與安全停機(jī)功能有效，確保產(chǎn)線安全運(yùn)行。

7.功能擴(kuò)展性：實(shí)時(shí)內(nèi)核支持C語(yǔ)言程序開(kāi)發(fā)，方便功能拓展與實(shí)時(shí)代碼提升效率。

MotionRT750視頻介紹可點(diǎn)擊→正運(yùn)動(dòng)強(qiáng)實(shí)時(shí)運(yùn)動(dòng)控制內(nèi)核MotionRT750。

更多關(guān)于MotionRT750的詳情介紹與使用點(diǎn)擊→強(qiáng)實(shí)時(shí)運(yùn)動(dòng)控制內(nèi)核MotionRT750(一)：驅(qū)動(dòng)安裝、內(nèi)核配置與使用。

超實(shí)時(shí)EtherCAT運(yùn)動(dòng)控制卡XPCIE6032H

XPCIE6032H運(yùn)動(dòng)控制卡集成6路獨(dú)立EtherCAT主站接口。整卡最高可支持254軸運(yùn)動(dòng)控制；125usEtherCAT通訊周期時(shí)，兩個(gè)端口配置冗余最高可支持32軸運(yùn)動(dòng)控制。6個(gè)EtherCAT主站各通道獨(dú)立工作，多EtherCAT主站互不影響。

XPCIE6032H視頻介紹可點(diǎn)擊→全球首創(chuàng)！PCIe 6路高性能EtherCAT運(yùn)動(dòng)控制卡XPCIE6032H。

XPCIE6032H運(yùn)動(dòng)控制卡面向半導(dǎo)體設(shè)備、精密3C電子、生物醫(yī)療儀器、新能源裝備、人形機(jī)器人及激光加工等高速高精場(chǎng)景，為固晶機(jī)、貼片機(jī)、分選機(jī)、鋰電切疊一體機(jī)、高速異形插件設(shè)備等自動(dòng)化裝備提供核心運(yùn)動(dòng)控制支持。

XPCIE6032H硬件特性：

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

2.板卡集成6路獨(dú)立的EtherCAT主站接口，最多可支持254軸運(yùn)動(dòng)控制。

3.搭載運(yùn)動(dòng)控制實(shí)時(shí)內(nèi)核MotionRT750。

4.相較于傳統(tǒng)的PCI/PCIe、網(wǎng)口等通訊方式，速度可提升了10-100倍以上。

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

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

更多關(guān)于XPCIE6032H的詳情介紹與使用點(diǎn)擊→全球首創(chuàng)！PCIe超實(shí)時(shí)6通道EtherCAT運(yùn)動(dòng)控制卡上市！。

超實(shí)時(shí)EtherCAT運(yùn)動(dòng)控制卡XPCIE2032H

XPCIE2032H集成2路獨(dú)立EtherCAT接口。整卡最高可支持至254軸運(yùn)動(dòng)控制；125usEtherCAT通訊周期時(shí)，單接口最高可支持32軸運(yùn)動(dòng)控制。2個(gè)EtherCAT主站各通道獨(dú)立工作，多EtherCAT主站互不影響。

雙EtherCAT主站端口可任意設(shè)置為以下通道，且兩個(gè)端口也設(shè)置為不同類(lèi)型通道:

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

● 常規(guī)通道 - EtherCAT通訊周期250us-8ms

XPCIE2032H視頻介紹可點(diǎn)擊→高速高精運(yùn)動(dòng)控制！PCIe超實(shí)時(shí)2通道EtherCAT運(yùn)動(dòng)控制卡上市！。

XPCIE2032H硬件特性：

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

2.板卡集成2路獨(dú)立的EtherCAT主站接口，最多可支持254軸運(yùn)動(dòng)控制。

3.搭載運(yùn)動(dòng)控制實(shí)時(shí)內(nèi)核MotionRT750。

4.相較于傳統(tǒng)的PCI/PCIe、網(wǎng)口等通訊方式，速度可提升了10-100倍以上。

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

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

更多關(guān)于XPCIE2032H的詳情介紹與使用點(diǎn)擊→高速高精運(yùn)動(dòng)控制！PCIe超實(shí)時(shí)2通道EtherCAT運(yùn)動(dòng)控制卡上市！。

超實(shí)時(shí)EtherCAT運(yùn)動(dòng)控制卡XPCIE1032H

XPCIE1032H是一款基于PCI Express的EtherCAT總線運(yùn)動(dòng)控制卡，可選6-64軸運(yùn)動(dòng)控制，支持多路高速數(shù)字輸入輸出，可輕松實(shí)現(xiàn)多軸同步控制和高速數(shù)據(jù)傳輸。

XPCIE1032H視頻介紹可點(diǎn)擊→高性能PCIe EtherCAT運(yùn)動(dòng)控制卡 | XPCIE1032H_。

XPCIE1032H運(yùn)動(dòng)控制卡集成了強(qiáng)大的運(yùn)動(dòng)控制功能，結(jié)合MotionRT7運(yùn)動(dòng)控制實(shí)時(shí)軟核，解決了高速高精應(yīng)用中，PC Windows開(kāi)發(fā)的非實(shí)時(shí)痛點(diǎn)，指令交互速度比傳統(tǒng)的PCI/PCIe快10倍。

XPCIE1032H硬件特性：

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

2.16軸EtherCAT同步周期500us，支持多卡聯(lián)動(dòng)。

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

4.通過(guò)EtherCAT總線，可擴(kuò)展到512個(gè)隔離輸入或輸出口。

5.支持PWM輸出、精準(zhǔn)輸出、PSO硬件位置比較輸出、視覺(jué)飛拍等。

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

7.支持電子凸輪、電子齒輪、位置鎖存、同步跟隨、虛擬軸、螺距補(bǔ)償?shù)裙δ堋?/p>

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

更多關(guān)于XPCIE1032H詳情點(diǎn)擊“不止10倍提速！PCIe EtherCAT實(shí)時(shí)運(yùn)動(dòng)控制卡XPCIE1032H 等您評(píng)測(cè)！”查看。

如何使用C語(yǔ)言與BASIC語(yǔ)言進(jìn)行配合

01固件版本要求

控制器使用C函數(shù)需要使用支持C接口函數(shù)的固件版本；固件版本名稱(chēng)里帶有“cfunc”的即為支持C函數(shù)接口。

02函數(shù)調(diào)用限制

同一個(gè)C文件內(nèi)的某一個(gè)C函數(shù)只能在某一個(gè)Basic文件內(nèi)被聲明調(diào)用，不可被在多個(gè)basic文件內(nèi)都進(jìn)行聲明調(diào)用。

03多函數(shù)聲明規(guī)則

同一個(gè)C文件內(nèi)的不同C函數(shù)可以分別在不同的Basic文件內(nèi)被聲明調(diào)用，但再次被聲明后的函數(shù)名不可一致。

DEFINE_CFUNC -- 關(guān)鍵字

支持的數(shù)據(jù)類(lèi)型定義：int、float、double、TYPE_TABLE。如果與TABLE數(shù)組交互，建議使用TYPE_TABLE類(lèi)型。在4系列以上的控制器，TYPE_TABLE是double類(lèi)型。

舉例一

C語(yǔ)言編程部分:

intuserc_init(void)
{
 int* p=(int*)malloc(sizeof(int));
   p[0]=88;
 printf("p[0]=%dn",p[0]);
 free(p);
 return0;
}
floatdivf(floata,floatb)
{
 return(a/b);
}
TYPE_TABLEdivd(TYPE_TABLE a,TYPE_TABLE b)
{
 return(a/b);
}

BASIC編程部分:

define_cfunc userc_initintuserc_init(void)
define_cfunc userc_divffloatdivf(floata,floatb)
define_cfunc userc_divddoubledivd(doublea,doubleb)
?userc_init()
?userc_divf(23.1,1)
?userc_divd(23.3,1)

以下程序代碼測(cè)試TABLE指針輸入輸出數(shù)據(jù)。

舉例二

C語(yǔ)言編程部分:

inttablefunc(TYPE_TABLE *ptable,intinum)
{
 inti;
 for(i=0;i

	

	BASIC編程部分:

	
define_cfunc userc_tfuncinttablefunc(TYPE_TABLE *ptable,intinum)
dimptable(20)
dim inum
userc_tfunc(ptable,inum)

	

	注意事項(xiàng)：

	（1）定義的無(wú)參數(shù)函數(shù)，可以在INT_CYCLE中直接使用。

	（2）BASIC調(diào)用的C函數(shù)的參數(shù)個(gè)數(shù)最多支持8個(gè)。

	（3）C函數(shù)注意安全性，注意代碼規(guī)范性，否則可能導(dǎo)致死機(jī)。

	（4）C函數(shù)要注意實(shí)時(shí)性，處理必須夠快，否則會(huì)影響B(tài)ASIC的實(shí)時(shí)性。

	注：建議調(diào)試時(shí)都下載RAM運(yùn)行！

	編譯平臺(tái)選擇參照

	不同型號(hào)控制器編譯平臺(tái)有所不同，具體參照下表。目前僅以下型號(hào)控制器支持C語(yǔ)言，其他系列型號(hào)控制器如有疑問(wèn)請(qǐng)與技術(shù)工程師聯(lián)系。

	

	右鍵單擊“文件視圖”中空白區(qū)域，點(diǎn)擊彈出窗口中“設(shè)置”一欄，進(jìn)行編譯平臺(tái)設(shè)置操作。在彈出窗口“編譯平臺(tái)”一欄中點(diǎn)擊下拉列表，在下拉列表中選擇相應(yīng)的編譯平臺(tái)后單擊“確定”，即可完成編譯平臺(tái)設(shè)置操作。

	

	C函數(shù)使用步驟

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

	

	2.新建項(xiàng)目成功后，在新建的項(xiàng)目下新建新的Basic文件，并且自定義相關(guān)Basic文件名。點(diǎn)擊確定Basic文件創(chuàng)建成功。

	

	3.新建.C文件。步驟可參照第二步，新建文件類(lèi)型時(shí)需選擇“C”。

	

	4.右鍵單擊“文件視圖”中空白區(qū)域，點(diǎn)擊彈出窗口中“設(shè)置”一欄，進(jìn)行編譯平臺(tái)設(shè)置操作。

	

	5.在彈出窗口“編譯平臺(tái)”一欄中點(diǎn)擊下拉列表，在下拉列表中選擇相應(yīng)的編譯平臺(tái)后單擊“確定”，即可完成編譯平臺(tái)設(shè)置操作，編譯平臺(tái)的選擇可參照上一章節(jié)“編譯平臺(tái)選擇參考”。

	

	6.編寫(xiě)C函數(shù)。編寫(xiě)C程序時(shí)需先進(jìn)行頭文件聲明步驟，然后再按照C語(yǔ)言編碼規(guī)范編寫(xiě)C函數(shù)。

	頭文件聲明語(yǔ)法：#include “xxxxx.h”?！皒xxx.h”為引用內(nèi)置函數(shù)的頭文件。如下圖所示，“.h”頭文件的文件路徑需與項(xiàng)目文件路徑一致。“zmcbuildin.h”文件可聯(lián)系正運(yùn)動(dòng)工程師獲取。

	

	7.在BASIC程序中使用C函數(shù)時(shí)需要使用關(guān)鍵字“DEFINE_CFUNC”對(duì)被使用C函數(shù)進(jìn)行引用定義，定義后在Basic文件中使用重新賦予的函數(shù)名即可調(diào)用該C函數(shù)。

	

	注：C函數(shù)代碼在RTSys中直接編輯即可，在其他軟件中編輯復(fù)制到RTSys中時(shí)可能會(huì)導(dǎo)致亂碼或格式錯(cuò)誤等問(wèn)題。

	簡(jiǎn)單示例

	1.聲明C函數(shù)接口（zmcbuildin.h）

	C語(yǔ)言：

	
// 添加自定義函數(shù)聲明
#include "zmcbuildin.h"
doublefast_sin()
{
 doublesum =0;
 for(inti=0; i

	

	2.BASIC綁定CFUNC

	Basic語(yǔ)言：

	
DEFINE_CFUNC c_sin_calcdoublefat_sin()
```

	

	3.混合編程調(diào)用

	Basic語(yǔ)言：

	
sum= c_sin_calc()

	

	自定義機(jī)械手模型實(shí)現(xiàn)

	已有的機(jī)械手類(lèi)型如下，更多類(lèi)型可以查看《正運(yùn)動(dòng)機(jī)械手手冊(cè)》。

	

	對(duì)于需要實(shí)現(xiàn)自定義機(jī)械手模型的需求，我們內(nèi)置C語(yǔ)言提供了CFRAME機(jī)械手結(jié)構(gòu)擴(kuò)展功能，用戶(hù)可通過(guò)此功能自定義機(jī)械手模型，C函數(shù)部分使用 usercframe.c文件內(nèi)的即可，進(jìn)行算法自定義。函數(shù)用法示例相關(guān)代碼介紹如下：

	C函數(shù)部分如下：

	正運(yùn)動(dòng)控制器內(nèi)置Frame1~Frame999的機(jī)械手類(lèi)型，用戶(hù)自定義機(jī)械手類(lèi)型為FRAME1000~FRAME65535。

	函數(shù)名自動(dòng)根據(jù)frame編號(hào)生成，下面C語(yǔ)言部分根據(jù)1000來(lái)生成舉例。

	功能：擴(kuò)展機(jī)械手frame類(lèi)型

	

	功能：frame初始化

	
//機(jī)械手每次正逆解回調(diào)執(zhí)行，用戶(hù)可以初始化內(nèi)部變量
// 輸入:
//    pzmc	      控制器描述結(jié)構(gòu)指針
//    pframe	    機(jī)械手基本狀態(tài)指針
//    pParaList Table 參數(shù)列表
// 返回值, 成功與否, 0-OK
********************************/
uint32SOFRAME_INIT1000(struct_soZmcDisp *pzmc, struct_soFrameStatus* pframe, TYPE_TABLE* pParaList)
{
  //初始化
  int16 i;
  //把connreframe的table數(shù)值賦值給臂長(zhǎng) 和關(guān)節(jié)脈沖
  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);
  //關(guān)節(jié)一圈脈沖
  g_soframeinfo[0].m_pulse1= 1000; //為了安全起見(jiàn) demo故意設(shè)置 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];
  //更新當(dāng)前機(jī)械手姿態(tài)
  pframe->m_iHand =0;//具體用戶(hù)自己編寫(xiě)計(jì)算機(jī)械手當(dāng)前實(shí)際姿態(tài)，姿態(tài)數(shù)值含義用戶(hù)自己定義，
  pf = (struct_userframeinfo *)pframe->m_pPrivate;
  if(NULL== pf)
  {
   return-1;
  }
  //打印輸出測(cè)試
  rtprintf("init %.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%dn",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);
  return0;
}

	

	功能：frame正解

	
//關(guān)節(jié)坐標(biāo)轉(zhuǎn)換世界坐標(biāo)
//計(jì)算焊槍末端坐標(biāo)
// 輸入:
//    pzmc	 控制器描述結(jié)構(gòu)指針
//    pframe	 機(jī)械手基本狀態(tài)指針
//    pfJointPulsein	輸入關(guān)節(jié)脈沖坐標(biāo)
// 輸出
//    pHand	  輸出當(dāng)前姿態(tài)
//    pfWorldout輸出units單位世界坐標(biāo)WPOS
// 返回值, 成功與否, 0-OK
********************************/
uint32SOFRAME_TRANS1000(struct_soZmcDisp *pzmc, struct_soFrameStatus* pframe, TYPE_FRAME*pfJointPulsein, int32 *pHand, TYPE_FRAME* pfWorldout)
{
  inti;
  doubleuj[6],pout[6];
  struct_userframeinfo *pf =NULL;
  pf = (struct_userframeinfo *)pframe->m_pPrivate;
  if(NULL== pf)
  {
   return-1;
  }
  //把輸入脈沖轉(zhuǎn)為角度
  uj[0] = *(pfJointPulsein +0) / pf->m_pulse1 ; //關(guān)節(jié)一 實(shí)時(shí)橫焊位置 度
  uj[1] = *(pfJointPulsein +1) / pf->m_pulse2 ; //關(guān)節(jié)二 實(shí)時(shí)高度位置 度
  uj[2] = *(pfJointPulsein +2) / pf->m_pulse3 ; //關(guān)節(jié)三 實(shí)時(shí)鐘擺角度 度
  uj[3] = *(pfJointPulsein +3) / pf->m_pulse4 ; //關(guān)節(jié)四 實(shí)時(shí)俯仰角度 度
  //轉(zhuǎn)弧度
  uj[0] = uj[0] /180* PI;
  uj[1] = uj[1] /180* PI;
  uj[2] = uj[2] /180* PI;
  uj[3] = uj[3] /180* PI;
  //正解過(guò)程......
  //計(jì)算姿態(tài)，返回姿態(tài)
  *pHand =0;
  //demo 計(jì)算出世界坐標(biāo)
  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] ;
  //打印輸出測(cè)試
  if(0== g_printflag)
  {
    rtprintf("trans input %.2f,%.2f,%.2f,%.2f, output ,%.2f,%.2f,%.2f,%.2fn",uj[0],uj[1],uj[2],uj[3],pfWorldout[0],pfWorldout[1],pfWorldout[2],pfWorldout[3]);
    g_printflag =1;
  }
  return0;
}

	

	功能：frame逆解

	
//世界坐標(biāo)轉(zhuǎn)換關(guān)節(jié)坐標(biāo)
// 輸入:
//   pzmc	 控制器描述結(jié)構(gòu)指針
//   pframe	 機(jī)械手基本狀態(tài)指針
//   pfWorldin	輸入世界坐標(biāo)units單位
//   ihand	 輸入坐標(biāo)對(duì)應(yīng)姿態(tài), -1表示使用當(dāng)前姿態(tài).
// 輸出：
//   pfJointPulseout	輸出關(guān)節(jié)脈沖坐標(biāo)
// 返回值, 成功與否, 0-OK
********************************/
uint32SOFRAME_RETRANS1000(struct_soZmcDisp *pzmc, struct_soFrameStatus* pframe, TYPE_FRAME *pfWorldin, int32 ihand, TYPE_FRAME* pfJointPulseout)
{
  inti;
  doubleuw[6],pout[6];
  struct_userframeinfo *pf =NULL;
  pf = (struct_userframeinfo *)pframe->m_pPrivate;
  if(NULL== pf)
  {
   return-1;
  }
  //把世界坐標(biāo)脈沖轉(zhuǎn)為mm和角度 pfWorldin 已經(jīng)是除以u(píng)nits的值了
  uw[0] = pfWorldin[0] ; // 位置 x  mm
  uw[1] = pfWorldin[1] ; // 位置 y  mm
  uw[2] = pfWorldin[2] ; // 位置 z  mm
  uw[3] = pfWorldin[3] ; // 位置 roll 弧度
  //轉(zhuǎn)弧度
  uw[3] = uw[3] /180*PI;
  //逆解過(guò)程......
  //計(jì)算出關(guān)節(jié)坐標(biāo)
  pout[0] = uw[0];//關(guān)節(jié)1  角度
  pout[1] =60;//關(guān)節(jié)2  角度
  pout[2] =60;//關(guān)節(jié)3  角度
  pout[3] =30;//關(guān)節(jié)4  角度
  //demo 為了安全起見(jiàn) 不改變關(guān)節(jié)輸出
  pfJointPulseout[0] = pout[0] *1000;
  pfJointPulseout[1] = pout[1] *1000;
  pfJointPulseout[2] = pout[2] *1000;
  pfJointPulseout[3] = pout[3] *1000;
  //打印輸出測(cè)試
  if(1== g_printflag)
  {
    rtprintf("retrans input %.2f,%.2f,%.2f,%.2f, output ,%.2f,%.2f,%.2f,%.2fn",uw[0],uw[1],uw[2],uw[3],pfJointPulseout[0],pfJointPulseout[1],pfJointPulseout[2],pfJointPulseout[3]);
    g_printflag =0;
 }
 return0;
}

	

	功能：自定義坐標(biāo)旋轉(zhuǎn)函數(shù)DPOS轉(zhuǎn)換為WPOS

	
// 輸入:
//   pzmc	 控制器描述結(jié)構(gòu)指針
//   pframe	 機(jī)械手基本狀態(tài)指針
//   pfRotate 坐標(biāo)平移旋轉(zhuǎn)的參數(shù), 依次為XYZ,RX,RY,RZ
//   pfin 	 輸入虛擬軸坐標(biāo)列表, units單位
// 輸出
//   pfout 	 輸出世界坐標(biāo)列表, units單位
// 返回值, 成功與否, 0-OK
********************************/
uint32SOFRAME_ROTATETOWPOS1000(struct_soZmcDisp*pzmc, struct_soFrameStatus*pframe,TYPE_FRAME*pfRotate,TYPE_FRAME*pfin,TYPE_FRAME*pfout)
{
  int i;
  rtprintf("DPOS轉(zhuǎn)換為WPOS: %0.4f, %0.4f, %0.4fn",*(pfRotate),*(pfRotate+1),*(pfRotate+3));
  // 輸入的就是世界坐標(biāo)系, 轉(zhuǎn)units坐標(biāo)系
  for(i=0; im_iTotalAxisesNoAux; i++)
  {
    pfout[i]=pfin[i];
  }
  rtprintf("pfin %0.4f, %0.4f, %0.4fn",*(pfin),*(pfin+1),*(pfin+3));
  rtprintf("pfout %0.4f, %0.4f, %0.4fn",*(pfout),*(pfout+1),*(pfout+3));
  return0;
}

	

	功能：自定義坐標(biāo)旋轉(zhuǎn)函數(shù)WPOS轉(zhuǎn)換為DPOS

	
// 輸入:
//   pzmc	 控制器描述結(jié)構(gòu)指針
//   pframe	 機(jī)械手基本狀態(tài)指針
//   pfRotate 坐標(biāo)旋轉(zhuǎn)的參數(shù)
//   pfin 	 輸入世界坐標(biāo)列表, units單位
// 輸出
//   pfout 	 輸出虛擬軸坐標(biāo)列表, units單位
// 返回值, 成功與否,0-OK
uint32 SOFRAME_ROTATETODPOS1000(struct_soZmcDisp *pzmc, struct_soFrameStatus* pframe, TYPE_FRAME *pfRotate, TYPE_FRAME *pfin, TYPE_FRAME* pfout)
{
  inti;
  rtprintf("WPOS轉(zhuǎn)換為DPOS: %0.4f, %0.4f, %0.4fn",*(pfRotate),*(pfRotate +1),*(pfRotate +3));
  //輸入的就是世界坐標(biāo)系, 轉(zhuǎn)units坐標(biāo)系
  for(i=0; i< pframe-??>m_iTotalAxisesNoAux; i++)
  {
    pfout[i] = pfin[i];
  }
    rtprintf("pfin %0.4f, %0.4f, %0.4fn",*(pfin),*(pfin +1),*(pfin +3));
  rtprintf("pfout %0.4f, %0.4f, %0.4fn",*(pfout),*(pfout +1),*(pfout +3));
  return0;
}


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

	

	該例子參數(shù)均為模擬，主要目的為介紹各函數(shù)接口使用方法。通過(guò)檢測(cè)輸入口0的狀態(tài)進(jìn)行正逆解切換：輸入口0上升沿觸發(fā)時(shí)進(jìn)入正解模式，輸入口0下降沿觸發(fā)時(shí)進(jìn)入逆解模式。實(shí)際使用時(shí)根據(jù)機(jī)械手參數(shù)進(jìn)行填寫(xiě)。

	機(jī)械手每次正逆解回調(diào)執(zhí)行都會(huì)執(zhí)行frame初始化函數(shù)，從參數(shù)表中讀取機(jī)械手的臂長(zhǎng)參數(shù)，設(shè)置關(guān)節(jié)脈沖參數(shù)(示例中設(shè)為固定值1000），保存參數(shù)到全局結(jié)構(gòu)體并關(guān)聯(lián)到機(jī)械手狀態(tài)，設(shè)置初始機(jī)械手姿態(tài)為0，打印初始化參數(shù)用于調(diào)試。

	進(jìn)入正解模式：通過(guò)函數(shù)SOFRAME_TRANS1000，將輸入的關(guān)節(jié)脈沖值轉(zhuǎn)換為角度值，將角度值轉(zhuǎn)換為弧度值，執(zhí)行正解運(yùn)動(dòng)學(xué)計(jì)算(示例中除關(guān)節(jié)軸0其他直接返回固定值)，設(shè)置輸出姿態(tài)，將計(jì)算結(jié)果轉(zhuǎn)換為世界坐標(biāo)系輸出，使用打印標(biāo)志控制調(diào)試信息的輸出頻率。

	進(jìn)入逆解模式：通過(guò)函數(shù)SOFRAME_RETRANS1000，將輸入的世界坐標(biāo)轉(zhuǎn)換為適當(dāng)單位，執(zhí)行逆解運(yùn)動(dòng)學(xué)計(jì)算(示例中直接返回固定值)，將計(jì)算結(jié)果轉(zhuǎn)換為關(guān)節(jié)脈沖值輸出，使用打印標(biāo)志控制調(diào)試信息的輸出頻率。

	注意事項(xiàng)：

	1.函數(shù)命名必須遵循約定：SOFRAME_前綴+功能名+Frame編號(hào)。

	2.確保正確處理單位轉(zhuǎn)換(脈沖?單位)。

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

	4.自定義旋轉(zhuǎn)功能僅在rotatetype≥100時(shí)需要實(shí)現(xiàn)。

	5.所有函數(shù)返回0表示成功，非0表示錯(cuò)誤。

	通過(guò)以上步驟，可以成功實(shí)現(xiàn)自定義機(jī)械手的CFRAME擴(kuò)展功能。

	需要更詳細(xì)的技術(shù)交流，請(qǐng)聯(lián)系正運(yùn)動(dòng)技術(shù)。

	教學(xué)視頻可點(diǎn)擊→強(qiáng)實(shí)時(shí)運(yùn)動(dòng)控制內(nèi)核MotionRT750(九)：內(nèi)置C語(yǔ)言的自定義機(jī)械手模型實(shí)現(xiàn)查看。

	本次，正運(yùn)動(dòng)技術(shù)強(qiáng)實(shí)時(shí)運(yùn)動(dòng)控制內(nèi)核MotionRT750(九)：內(nèi)置C語(yǔ)言的自定義機(jī)械手模型實(shí)現(xiàn)，就分享到這里。

	本文由正運(yùn)動(dòng)技術(shù)原創(chuàng)，歡迎大家轉(zhuǎn)載，共同學(xué)習(xí)，一起提高中國(guó)智能制造水平。文章版權(quán)歸正運(yùn)動(dòng)技術(shù)所有，如有轉(zhuǎn)載請(qǐng)注明文章來(lái)源。


	審核編輯 黃宇