Omron CK3M-CPU 1 Series Application Manual page 27

13
Write the text shown on the right to the
usercode.c file.
14
Add the text shown on the right to the top of
the usercode.c file.
Application Guide Laser Application (O052)
static double prevLpfXPos=0;
static double prevLpfYPos=0;
double CfromScript(double kinTypeDouble,double ar
g2,double arg3,double arg4,double arg5,double arg
6,double arg7,LocalData *Ldata)
{
int kinType=(int)kinTypeDouble;
double deltaXPos,deltaYPos;
double *L=GetLVarPtr(Ldata);
double *C=GetCVarPtr(Ldata);
switch(kinType)
{
case Forward_Kinematics_State:
{
_KinPosAxisX=_KinPosMotor1+(_KinPosMotor3/Galv
oSfX);
_KinPosAxisY=_KinPosMotor2+(_KinPosMotor4/Galv
oSfY);
if(Ldata->Status & 0x40)
{
prevLpfXPos=_KinPosMotor1;
prevLpfYPos=_KinPosMotor2;
}
break;
}
case Inverse_Kinematics_State:
{
deltaXPos=_KinPosAxisX-prevLpfXPos;
deltaYPos=_KinPosAxisY-prevLpfYPos;
_KinPosMotor1=prevLpfXPos+(KdgainX*deltaXPos);
_KinPosMotor2=prevLpfYPos+(KdgainY*deltaYPos);
_KinPosMotor3=(_KinPosAxisX-_KinPosMotor1)*Gal
voSfX;
_KinPosMotor4=(_KinPosAxisY-_KinPosMotor2)*Gal
voSfY;
prevLpfXPos=_KinPosMotor1;
prevLpfYPos=_KinPosMotor2;
break;
}
}
return 0;
}
#define Forward_Kinematics_State 0
#define Inverse_Kinematics_State 1
#define _KinPosAxisX *(C+6)
#define _KinPosAxisY *(C+7)
#define _KinPosMotor1 *(L+1)
#define _KinPosMotor2 *(L+2)
#define _KinPosMotor3 *(L+3)
#define _KinPosMotor4 *(L+4)
3 Connection Procedures
3
3-11