CK3M自定义伺服算法开发的流程是怎样的？

在导师的要求下，对欧姆龙的CK3M控制卡进行自定义伺服算法（C语言）二次开发。这也是我第一次接触CK3M，文章里面还有很多错误我未能察觉，还请大家批评指正
由于较少登录使用CSDN账号，此文章同时发布在我的知乎账号Stan里，之后相关内容更新都在该知乎文章里☞CK3M自定义伺服算法开发的简单流程。如有需要也欢迎转载，并请注明出处~ ///(^ v ^)\ ~
  




1 注意事项

该手册主要介绍OMRON的PMAC(CK3M)运动控制卡的自定义伺服算法（C语言）开发与运动程序。PMAC在运动控制卡领域和ACS、Trio排在第一梯队，这个品牌的控制器网上资源很少，国内厂家技术也相对较为缺乏，控制卡的自定义伺服算法更是很少人对其进行开发。就Power PMAC（CK3M）而言，比较推荐大家使用的资料是官方的《Power PMAC 5-Day Training》，这份文档很全面地介绍了PMAC的使用，无论零基础入门还是快速进阶都是很好的教材。如果在使用过程中遇到复杂问题，可以进一步查阅官方手册《Power PMAC IDE》、《Power PMAC Software Reference Manual》、《Power PMAC Users Manual》，以及国内代理商——北京泰诺德科技有限公司的《POWER PMAC初级技术培训》。（我把这5份官方文档上传到了CSDN，如有需要可前往下载）





CK3M有5种控制方式可供选择，我们实验室使用的是直接PWM控制，反馈模块采用AX-2323N模拟量反馈，PWM放大器采用Delta Tau公司定制的CDHD-0062APB0高创驱动器（该驱动器无其他功能，就是单纯的PWM放大器），PowerPMAC IDE版本是4.3.2.19（CK3M与Power PMAC通用）。驱动器手册参考《CDHD PW -PWM Power Block Preliminary Specification》。










[tr]CDHD-0062APB0高创驱动器（PWM放大器）[/tr]
对于Direct PWM模式，PMAC包含了位置环+速度环+电流环。这种全数字的控制模式：

• 可以减小如模拟量控制的A/D、D/A转换产生的延时，信号传输出现的噪声干扰等；
  
• 位置环+速度环+电流环均在控制器内部，减小了各个环路数据交互和计算的延时。更低的延时允许更高的增益，可以提高系统的带宽，刚性及加速能力。
  
  

使用前的几个提醒：

• 通讯连接，首先更改电脑IP地址为：
  
  
  
  
  
  
  
  
  
  
• 终端输入指令不能是中文
  
• 配置文件在PMAC Script Language-Global Includes-global definitions，也可以从CK3M的内存里上传项目到IDE
  
• 有时候如果需要断开PC与CK3M的通讯，可以点击通讯设置，选择无设备
  
• 首次寻相需要在终端里面输入指令：system/opt/ppmac/setup/calcadcoffset 1，并复制Motor[1].IaBias =（终端里面显示的数字），Motor[1].IbBias=（终端里面显示的数字）
  
• 指令save是将配置文件保存到控制器，然后在输入$$$（作用是断电重启，让变量能生效）
  
• Power PMAC与CK3M的区别在于引脚定义不同，但指令是相同的
  
• 在项目栏-属性中，可以更改M变量、P变量和Q变量的起始地址，
  
  
  
  
  
  这里将P变量起点设置为0，即P变量从P[0]开始分配地址。
  
  

2 自定义伺服算法（C语言）开发

首先，新建一个项目，选择PowerPMAC，





对于自定义伺服算法，我们需要用到的三个文件是：资源管理器-C Language-Realtime Routines-usrcode.c和usrcode.h，PMAC Script Language-Global Includes-global definitions.pmh





2.1 Global Includes

新项目中的global definitions.pmh文件是空白的，在这里定义（为了便于阅读理解，有关参数说明都写在代码注释里）——

Sys.WpKey = $AAAAAAAA  //关闭写保护
Sys.MaxCoords = 3
Sys.MaxMotors = 8


//ADRC算法控制变量
GLOBAL z1=0,z2=0,z3=0,e1=0,_u=0,_u0=0,sh=1/8000,uout=0,_actvel=0,_actpos=0,_poserror=0,_desvel=0;
GLOBAL wc=60,wo=500,xi=1,b0=3,switch_control=1;//这里定义的是P变量，每伺服周期更新一次；地址从P[0]开始，sh为CK3M伺服更新周期，1/8000=0.125ms；同时初始化控制器参数
//ptr wc->Sys.Ddata[1];
//ptr wo->Sys.Ddata[2];
//ptr xi->Sys.Ddata[3];
//ptr b0->Sys.Ddata[4];
//ptr _kpp->Sys.Ddata[5];
//ptr _kdd->Sys.Ddata[6];


Gate3[0].PhaseFreq = 16000//IC内部相位时钟频率16kHz
Gate3[0].ServoClockDiv = 1//SERVO = PhaseFreq / (ServoClockDiv + 1) 8kHz
Sys.RtIntPeriod = 0//RTI = ServoFreq / (RtIntPeriod + 1)
Sys.ServoPeriod = 1000 * (Gate3[0].ServoClockDiv + 1) / Gate3[0].PhaseFreq //CK3M伺服更新周期，以ms为单位，1/8000=0.125ms
Sys.PhaseOverServoPeriod = 1 / (Gate3[0].ServoClockDiv + 1)//相位与伺服更新周期的比率
//CDHD-PB驱动器（实验室的高创驱动器型号是CDHD-0062APB0）规定PWM频率不能超过8KHz（部分型号为16Khz），且由于PWM频率最小只能设置为Phase频率的一半，因此Phase频率不能超过16kHz


//============================输出模式配置============================//
Gate3[0].AdcAmpHeaderBits=0    //=0 CDHD
Gate3[0].EncClockDiv=3// 设置编码器采样时钟
ECAT[0].ServoExtension = 15
Gate3[0].Chan[0].PwmFreqMult = 0  //PWM : 8KHz，FREQ = PhaseFreq*(PwmFreqMult + 1)/2
Gate3[0].Chan[0].PackOutData = 1//PWM/DAC通道pack使能
Gate3[0].Chan[0].PackInData = 2
Gate3[0].Chan[0].CaptFlagChan=0   //原点相关参数//捕获标志通道选择
Gate3[0].Chan[0].CaptFlagSel=0   //原点相关参数//位置伺服通道选择
Gate3[0].Chan[0].CaptCtrl=1      //原点相关参数//通道位置捕获控制（在索引高时捕获）
Gate3[0].Chan[3].EncCtrl=3//伺服IC通道编码器解码控制（四倍频）
Gate3[0].Chan[0].GatedIndexSel=1//由正交通道控制的索引进行定位


//============================编码器转换表配置，AX - 2323N，1VPP 模拟量反馈============================//
EncTable[1].type = 1//编码表项转换方法（单字32位读）
EncTable[1].pEnc = Gate3[0].Chan[0].ServoCapt.a//编码器表项的主原地址
EncTable[1].pEnc1 = Sys.pushm
EncTable[1].index1 = 0//编码器表项第一转换因子
EncTable[1].index2 = 0
EncTable[1].index3 = 0
EncTable[1].index4 = 0
EncTable[1].index5 = 0
EncTable[1].index6 = 0
EncTable[1].ScaleFactor = 1    //编码器表输入输出比例系数


//======================================== Motor1 配置参数=====================================//
Motor[1].AdcMask = $FFF00000//电流反馈位通掩码字，CDHD-PB 是14bits ADC 但实际只有12bits有数值，
Motor[1].AmpFaultLevel = 1//放大器故障逻辑状态
Motor[1].PhaseCtrl = 1
Motor[1].ServoCtrl=1//ACTIVATE CHANNEL
//Motor[1].Ctrl=Sys.ServoCtrl//指向选定伺服算法的指针
Motor[1].Ctrl=UserAlgo.ServoCtrlAddr[1]//指定自定义伺服算法


GLOBAL DcBusInput = 340;    //驱动器直流母线电压，母线电压交流220V时，对应340V -–用户输入
GLOBAL Mtr1DCVoltage = 320; //电机直流最大承受电压，（略小于母线电压）或根据电机参数设置 -–用户输入
Motor[1].PwmSf = -16384 * Mtr1DCVoltage / DcBusInput
Gate3[0].Chan[0].EncCtrl =3
Motor[1].PhaseOffset = -683


/*************************Motor1 I2T电流保护参数************************/
GLOBAL Ch1MaxAdc = 18; // 驱动器ADC最大电流（根据驱动器手册获得）[A rms]
GLOBAL Ch1RmsPeakCur = 9.2; // 电机RMS 峰值电流 [A rms] -–用户输入
GLOBAL Ch1RmsContCur = 1.8; // 电机RMS 连续电流 [A rms] --用户输入
GLOBAL Ch1TimeAtPeak = 0.2; // 电机峰值电流允许持续时间，单位s
Motor[1].MaxDac = Ch1RmsPeakCur * 40132.44 / Ch1MaxAdc//设置I2T电流保护
Motor[1].I2TSet = Ch1RmsContCur * 40132.44 / Ch1MaxAdc
Motor[1].I2tTrip = (POW(Motor[1].MaxDac,2) - POW(Motor[1].I2TSet,2)) * Ch1TimeAtPeak


/*************************Motor1电流环参数************************/
//Motor[1].IiGain = 0.1// 电流环积分增益 根据实际情况调试-–用户输入
//Motor[1].IpfGain = 3.0//电流环前向比例增益 根据实际情况调试 -–用户输入
//Motor[1].IpbGain=0  //电流环反馈比例增益 根据实际情况调试 -–用户输入
Motor[1].IaBias =47 //A相电流ADC偏移量（根据Motor[x].IaMeas调节，使其为0） -–用户输入31
Motor[1].IbBias=-31 //B相电流ADC偏移量（根据Motor[x].IbMeas调节，使其为0） -–用户输入-63


/*************************Motor1换相参数************************/
Motor[1].PhasePosSf = 2048/(16384*60*25)   //1.312335958005249e-05 //根据电机极距或极对数计算，= 2048/(1个极距对应的脉冲数 *256 )，若为模拟量反馈，无需乘256，在这里电机极距为60mm，栅距为40um的模拟量光栅尺，AX-2323N反馈卡细分为16384 Per Line，每mm脉冲数为25*16384 cts/mm
Motor[1].pPhaseEnc = Gate3[0].Chan[0].PhaseCapt.a //电机相位信息获取地址


/*************************Motor1寻相参数************************/
Motor[1].PhaseFindingDac = Motor[1].I2tSet*0.5  //寻相电流，一般为Motor[x].I2TSet一半
Motor[1].PhaseFindingTime = 2000*Sys.ServoPeriod//寻相时间
Motor[1].AbsPhasePosOffset = 2048/5


/*************************Motor1常规参数************************/
Motor[1].pEnc=EncTable[1].a// POSITION FEEDBACK POINTER
Motor[1].pEnc2=EncTable[1].a// VELOCITY FEEDBACK POINTER       
Motor[1].pLimits=0//Gate3[0].Chan[0].Status.a，=0 TO DISABLE
Motor[1].LimitBits=9
Motor[1].pAmpFault=Gate3[0].Chan[0].Status.a
Motor[1].AmpFaultBit=7
Motor[1].PosSf=1/(25*16384)//设置反馈分辨率，单位：mm/cts，AX-2323N模拟量反馈卡细分为2^14=16384 Per Line；我们的模拟量光栅尺栅距为40μm，因此1mm有25线--> 25*16384 cts/mm
//若要修改模拟量光栅尺的细分倍数，只需将2^14改为2的其他比14小的幂，如2^13=8192，此时其他语句中的16384也得改为8192
Motor[1].Pos2Sf=Motor[1].PosSf// SAME ENCODER
Motor[1].pDac = Gate3[0].Chan[0].Pwm[0].a
Motor[1].pAmpEnable = Gate3[0].Chan[0].OutCtrl.a
Motor[1].AmpEnableBit = 8


Motor[1].JogSpeed=0.1
Motor[1].JogTa=100
Motor[1].JogTs=20
Motor[1].HomeVel=-0.015
Motor[1].MinPos=0
Motor[1].MaxPos=0
Motor[1].FatalFeLimit=10000
Motor[1].WarnFeLimit=10000
Motor[1].MaxSpeed=1//最大编程速度幅度，单位是电机单位/ms，默认值是32，在调试算法时可先将此值设置成一个较小值，确保在出现意外时减小危险


/*************************Motor1伺服参数************************/
//Motor[1].Servo.Kp=600
//Motor[1].Servo.Kvfb=500
//Motor[1].Servo.Kvff=500
//Motor[1].Servo.Ki=0.00005


/*************************Motor1编码器丢失检测参数************************/
Motor[1].EncLossLevel=1
Motor[1].EncLossLimit=255
Motor[1].pEncLoss=0//Gate3[0].Chan[0].Status.a
Motor[1].EncLossBit=28


/*************************Motor1急停参数参数************************/
Motor[1].AbortTa=200
Motor[1].AbortTs=30




Gate3[0].Chan[0].OutputMode=0;
Gate3[0].Chan[0].EncCtrl=7//伺服IC通道编码器解码控制（四倍频）
Gate3[0].Chan[0].AtanEna = 1


//===============反馈配置，AX - 2323N，1VPP 模拟量反馈==============//
Gate3[0].AdcEncCtrl = $3FFFC000
Gate3[0].AdcEncDelay = 0
Gate3[0].AdcEncHeaderBits = 0
Gate3[0].AdcEncStrobe = $3FFFC0
Gate3[0].AdcEncUtoS = 0





可以看到usrcode.c文件的左边有一个小红点，在程序编译之前需要先将其设置为编译模式：右键usrcode.c，选择属性，将Build Action设置为Compile，





如果在usrcode.c上的build操作被设置为Compile，IDE会识别出用户正在使用实时C例程。
由于实验室项目需要，以下伺服算法采用的是ADRC控制。关于ADRC，这里不再做相关介绍，但为了方便大家理解，我搭建了一个简单的Simulink框图以供参考。





[tr]ADRC控制算法Simulink框图[/tr]






[tr]ESO的Simulink框图[/tr]
限幅设为±10，输入指令为阶跃信号，阶跃时间为1，阶跃值为10，控制器参数wc=10;wo=100;b0=2;xi=1;





[tr]仿真结果[/tr]

采用自定义伺服算法，需要先在usrcode.c文件中添加下列函数声明，算法需要用到的全局变量和运算的中间变量可在global definitions.pmh文件中声明为P变量：


double user_pid_ctrl(struct MotorData *Mptr)
{
// Contents of servo algorithm
return 0.0; // Return value (change this to
// your algorithm’s actual return
// value)
}


然后在usrcode.h头文件中添加下列行：


// Function prototype
double user_pid_ctrl(struct MotorData *Mptr);
EXPORT_SYMBOL(user_pid_ctrl); // Symbol exportation


注意：控制算法得到的反馈和计算的控制律都是模拟值，但是控制卡实际输出的是±32768以内的数字量，该值输出到驱动器，对应转换为驱动器的输出电流，电流经过电机才转换为力，因此有一个线性关系转换，参考《CDHD PW -PWM Power Block Preliminary








[tr]CDHD – PB 参数表[/tr]






[tr]示意图[/tr]
               
以LADRC（自抗扰控制）算法为例，在usrcode.c文件中输入代码：

double adrc(struct MotorData *Mptr)
{
_desvel=(Mptr->DesVel/sh)/1000;  //Motor[x].DesVel表示当前伺服周期的净期望速度值，即当前伺服周期的期望位置值与前一个伺服周期的期望位置值之差。
_actvel=(Mptr->ActVel/sh)/1000;  //因此获得速度值还需要除以伺服周期，除以1000是将单位mm转化为m。Motor[x].DesVel同理
_actpos=(Mptr->ActPos - Mptr->HomePos)/1000;//为了计算相对于电机零位的位置，需要减去Motor[x].HomePos的值。即#1hmz可以让_accpos为0。将单位mm转化为m
_poserror=(Mptr->PosError)/1000;
  //open-loop routine
  if(Mptr->ClosedLoop)
  {
    //LESO，一阶欧拉法计算积分
    e1 = (_actpos - z1);
    z1 = (3*wo*e1 + z2)*sh + z1;
    z2 = (3*pow(wo,2)*e1 + z3 + b0*_u)*sh + z2;
    z3 = (pow(wo,3)*e1)*sh + z3;
        //PD
//        _kpp = wc*wc;
//        _kdd = 2*xi*wc;
        _u0 = wc*wc*(_poserror) + (2*xi*wc)*(_desvel*switch_control - _actvel);
        _u  = (_u0 - z3 ) / b0;
        if(_u>1)
        {
                _u=1;
        }
        if(_u<-1)
        {
           _u=-1;
        }//电流限幅1Amps Peak
        uout = (_u*32768)/18/sqrt(2)/1.25;//_u乘以最大输出值2^15，除以驱动器最大电流（峰值），再除以1.25，转换为数字量。uout(控制卡输出的数字量)/32768(控制卡输出的最大值)=_u(计算得到的模拟量电流)/31.8(ADC电流峰值)
//注意：控制算法得到的反馈和计算的控制律都是模拟值，但是控制卡实际输出的是±32768以内的数字量，该值输出到驱动器，对应转换为驱动器的输出电流，电流经过电机才转换为力，因此有一个线性关系转换
//        return 0.0;//令算法输出值恒为0，可以藉此检查观测器或其他控制算法在线时是否会出现发散等不稳定现象。
        return uout;
  }
  else
  { // Open loop mode
  z1=_actpos,z2=0,z3=0,e1=0,_u=0,_u0=0,uout=0;//初始化观测器和控制器参数，积分一定要清零，注意z1初始值最好与actpos相等。
  return 0.0; // Zero output
  }
}


然后在global definitions.pmh文件中声明变量：


//ADRC算法控制变量
GLOBAL z1=0,z2=0,z3=0,e1=0,_u=0,_u0=0,sh=1/8000,uout=0,_actvel=0,_actpos=0,_poserror=0,_desvel=0;
GLOBAL wc=60,wo=500,xi=1,b0=3,switch_control=1;//这里定义的是P变量，每伺服周期更新一次；地址从P[0]开始，sh为CK3M伺服更新周期，1/8000=0.125ms；同时初始化控制器参数


最后在usrcode.h头文件中输入代码：


double adrc(struct MotorData *Mptr);
EXPORT_SYMBOL(adrc);


2.3 编译并下载程序

下一步，右键单击Realtime Routines并选择User Servo Setup。选择将要执行伺服算法的电机，选择用户伺服名称，然后点击应用，










右键单击项目，然后单击“Build and Download”。若程序没有错误，则输出窗口显示下载成功。当电机激活和使能之后，所选电机的用户伺服算法将开始运行。要验证电机是否使用了用户伺服算法，请检查Motor[1].Ctrl的值。如果它被设置为UserAlgo.ServoCtrlAddr，即是使用了用户伺服算法。





至此，自定义伺服算法已部署到CK3M中。完成上述操作后一定要保存和复位。在Terminal输入save，保存完成后再输入 $$$ 进行复位。在Terminal输入#1$进行寻相，然后输入#1j/使能电机，状态栏可以查看系统状态：





2.4 控制器调试

下一步是控制器的调试，可以在点动功能区进行（点击点动之后电机将自动使能），先点击Homez将初始位置置0（或输入#1hmz指令）,




同时要打开绘图功能，采集需要关注的数据，例如指令位置和实际位置，也可以查看所定义的P变量，





点击“收集数据”，同时点动20单位（mm），注意Watch里面的数据，










[tr]位置曲线[/tr]上载数据并绘制，如图，点击点动功能区的“终止”去使能，或者在Terminal输入k去使能。
特别注意：为了避免由于算法错误而导致飞车等危险事件发生，在给电机使能之前，可以将usrcode.c文件中的return值改为0.0，





此时算法输出值恒为0，可以藉此检查观测器或其他控制算法在线时是否会出现发散等不稳定现象。
使用.pmh文件中创建的变量名来实时调整控制器参数。例如，指令wc=44在终端窗口中设置wc参数为44。
3 运动程序






运动程序位于Motion Programs-prog1.pmc文件中，初始的prog1.pmc文件已经有程序的起始和结束语句，需要编写运动时间、加速时间等运动规划指令。也可以在程序中实现“打开采集”和“关闭采集”功能，方便在控制器调试的时候提高效率、而不需要在点动功能区和绘图两个窗口之间来回切换。下面是一个对称七段S形速度规划，并可实现自动采集数据：

/*For more information see notes.txt in the Documentation folder */


//Open prog Myprog1
// --------------------User Code Goes Here------------------------
// 运动程序 – 将运动程序粘贴到“prog1.pmc”中
// 在终端窗口输入"#1j/"使能，"&1b Exercise1 r" // 闭环, 坐标系1, 指向程序Exercise1的开始(Beginning), 运行(Run)
//undefine all// 清除所有坐标系下的轴定义
&1 // 使用“&”寻址坐标系1
#1->X  // 在坐标系1下使用 “#”寻址电机1，使用“->”将电机1分配给X轴
                      // 在这里轴X的比例因子是1，比例因子可以视为每个用户单位的脉冲计数
                      //  （单位counts，需要结合具体的光栅尺分辨率）等于 1 mm
open prog Exercise1 // 1.start motion program（打开缓冲区用户程序输入，Exercise1）
  homez 1 // 2.home motor #1(电机 #1回零)
  dwell 0
  Gather.Enable = 2 // 3.Gather.Enable=2 (打开采集)
  dwell 0
  linear // 4.set move modes（混合直线插补运动模式）
  abs // 定义位置编程模式，选择绝对式或增量式
  tm 900 // 5.set move time（以ms为单位）
  ta 100 // 6.set acceleration time（总运动时间为 TM + TA）
  X 10 // 7.move x 10 centimeters (在1 sec内移动到10个用户单位(mm))
  dwell 4000 // 8.wait 4 seconds (等待4 sec)
  X 0 // 9.move x to origin (在1 sec 内移动到起始点)
  dwell 0
  Gather.Enable = 0 // 10.Gather.Enable=0 (关闭采集)
  dwell 0 // 在程序结束时强制停止采集(这是必须的)
close // 11.end
在程序结束时强制停止采集(这是必须的)close // 11.end 编译并下载所有程序之后，打开绘图区，选择需要采集的数据，





然后在Terminal输入#1j/使能，再输入&1b Exercise1 r 运行运动程序。此时CK3M中的控制算法是Motor[1].Ctrl参数决定的，Motor[1].Ctrl=UserAlgo.ServoCtrlAddr[1]即是采用自定义伺服算法控制电机运动。



[tr]位置曲线[/tr]


[tr]加速度曲线[/tr]至此，完成了CK3M的自定义伺服算法开发的简易流程。