PMAC CfromScript函数功能说明

时间:2022-07-05 12:33:54


一,简介:

CfromScript功能是从PMAC脚本程序调用C语言的一种函数。这个功能允许用户从PMAC脚本语言直接访问C语言的子程序,其主要用在运动学计算,这个功能用户在更多不同的使用目的也可以灵活地的支持。这个功能对运动学计算特别适用(运动学的计算量通常都会比较大),因为相对于将计算直接写在基于脚本的子程序中,C语言的执行速度会更快。

 

Power PMAC只能有一个单独的CfromScript函数。但是,通过传递是有可能的从多个脚本程序调用这个单一函数的参数和内部逻辑,即使是在同一时间。

 

二,声明CfromScript()

CfromScript函数必须在“usrcode.c”中编写。位置在“RealtimeRoutines”项目管理的文件夹。

声明形式:

double CfromScript(double arg1, double arg2, double arg3, double arg4,double arg5,double arg6, double arg7, LocalData *Ldata)

 

用户可以给8个参数指定任何名称,但必须有7个参数的类型为“double”,以及最后一个为“LocalData”的参数结构类型。

usrcode.h”必须在相同的文件夹中,有完全匹配的定义和符号输出声明:

double CfromScript(double arg1,double arg2,doublearg3,doublearg4,double arg5,double arg6,double arg7,LocalData *Ldata);

EXPORT_SYMBOL(CfromScript);

 

三,从脚本程序调用CfromScript

         CfromScript()通常来自于在实时中断中执行的脚本子程序,例如前述PLC程序(sys.maxrtplc 所设置的数字),运动学子程序或运动程序。

 

这个函数可以从后台程序中调用。如果用户首先设置非保存的设置元素UserAlgo.CFunc1。并且用户想调用CfromScript()来自一个后台子程序,那么建议在项目管理中的“PMAC Script Language”-> “Global Includes”->“globaldefinitions.pmh”文件中设置UserAlgo.CFunc = 1

脚本程序中的调用命令格式:

MyReturnVar = CfromScript(arg1, arg2, arg3, arg4,arg5, arg6, arg7);

 

调用程序必须传递给CfromScript()Double类型的7个参数,实际上如果CfromScript()没有在内部使用所有的参数,或者如果CfromScript()没有使用其中的一个参数,建议将一个0传递给CfromScript()

 

Power PMAC(PQLRCD)中所有通用的用户变量都是double类型的。一些数据结构元素是double类型的,但是不同类型的元素的值可以通过将值复制到一个通用函数中,从而转换为double变量。

 

Power PMAC自动将CfromScript()8个参数的指针去调用程序的本地数据结构功能。用户应该在函数调用中不包含这个参数。

 

调用程序必须将CfromScript()的结果存储在一个变量中(RLCDPQM变量)即使结果是不需要的。否则,命令将被标记为语法错误。

 

注意,调用程序的执行,直到CfromScript()函数调用已经完成将停止,不需要编写额外的代码来强制PMAC等待CfromScript()来完成。

 

调用例程:

这个例子简单地调用前台PLC 0中的CfromScript(),将0传递给所有函数的参数,并将结果存储在P1000中。

open plc 0

P1000 =CfromScript(0,0,0,0,0,0,0);

Close

 

四,在CfromScript中使用本地数据变量

如果用户需要在CfromScript()中使用本地变量,可以通过本地变量数据指针通常被称为“Ldata”,它是内置的C函数,它将指针返回到LocalData空间中的RLCD变量数组。GetRVarPtr(),GetLVarPtr()getdvptr()GetDVarPtr()这些函数,用于RLCD的局部变量,请参阅下面的语法示例:

 

double CfromScript(double arg1, double arg2, double arg3 double arg4,

double arg5, double arg6, double arg7, LocalData *Ldata)

{

double *R;

double *L;

double *C;

double *D;

R = GetRVarPtr(Ldata);//Ldata->L + Ldata->Lindex + Ldata->Lsize;

L = GetLVarPtr(Ldata);//Ldata->L + Ldata->Lindex;

C = GetCVarPtr(Ldata);//Ldata->L + Ldata->Lindex + MAX_MOTORS;

D = GetDVarPtr(Ldata);// Ldata->D;

// User places additionalcalculations here

return 0.0; // Canchange this to return anything else if needed

}

一旦这些指针被赋值,RLCD变量就可以用数组符号来表示。例如,R[0]将等同于在调用CfromScript()的脚本程序中访问R0L[0]等同L0C[0]C0一样的,而D[0]D0相同,R[1]同理与R1相同,其他以此类推。

 

在运动学例程中,变量L[n]是马达n的位置,输入正运动学,以及逆运动学的结果。变量C[n]是坐标轴的位置—输入逆运动学,以及正运动学的结果。

 

 

五,从多个脚本程序调用CfromScript

         如果用户希望使用CfromScript()来实现许多不同的目的,那么可以设计CfromScript()成为一个状态机类型的处理函数。然后调用程序就简单了:需要向CfromScript()将状态数传递到它的7个可用参数中的一个,可能还有其他有用的数据,比如呼叫程序的坐标系统号。CfromScript()将根据状态调用适当的后续C函数它接收信息。

 

示例:使用CfromScript()作为一个运动学处理程序

这个例子使用了CfromScript()来执行来自多个坐标系统的正向和逆运动学。它使用从调用脚本运动学例程传递给它的参数来决定要采取什么操作。在本例中,程序判断要调用哪个C子程序,并且实际计算在这些子程序中完成。

 

下面的代码被放置在usrcode.c:

// #defines – For determiningthe states and kinematics types to use

#defineForward_Kinematics_State 0

#defineInverse_Kinematics_State 1

#define KinematicsType1 1

#define KinematicsType2 2

// Prototypes

int ForwardKinematics(int CS_Number,int Kinematics_Type, LocalData *Ldata);

int InverseKinematics(int CS_Number,int Kinematics_Type, LocalData *Ldata);

int ForwardKinematicsSubroutine1(int CS_Number,LocalData *Ldata);

int InverseKinematicsSubroutine1(int CS_Number,LocalData *Ldata);

int ForwardKinematicsSubroutine2(int CS_Number,LocalData *Ldata);

int InverseKinematicsSubroutine2(int CS_Number,LocalData *Ldata);

double CfromScript(double CS_Number_double,double State_double,double

KinematicsType_double,double arg4,double arg5,double arg6,double arg7,LocalData

*Ldata)

{

// CfromScript() functions asa State Machine handler.

// Inputs:

// CS_Number_double:Coordinate System number of the coordinate system

// program that called thisinstance of CfromScript().

// State_double: Statenumber. Pass in the state corresponding to

// what the user wantsCfromScript() to do; e.g., design CfromScript() such

// thatForward_Kinematics_State ( = 0) will make CfromScript() run the

// forward kinematicsroutine.

// KinematicsType_double:Type of kinematics to use. Only need to use this

// argument if usingkinematics; otherwise, set to 0.

// arg4 - arg7: unused inthis example.

// Output: ErrCode – errorcode of function calls.

// Will return -11 if invalidstate entered.

int CS_Number = (int)CS_Number_double;

intState = (int)State_double;

int KinematicsType = (int)KinematicsType_double;

int ErrCode = 0;

switch(State)

{

case Forward_Kinematics_State:

{

ErrCode =ForwardKinematics(CS_Number,KinematicsType,Ldata);

break;

}

case Inverse_Kinematics_State:

{

ErrCode =InverseKinematics(CS_Number,KinematicsType,Ldata);

break;

}

default:

{

ErrCode = -11; // InvalidState Entered

break;

}

}

return (double)ErrCode;

}

int ForwardKinematics(int CS_Number,int Kinematics_Type, LocalData *Ldata)

{

int ErrCode = 0;

switch(Kinematics_Type)

{

case KinematicsType1:

ErrCode =ForwardKinematicsSubroutine1(CS_Number,Ldata);

break;

case KinematicsType2:

ErrCode =ForwardKinematicsSubroutine2(CS_Number,Ldata);

break;

// Can implement other typesof forward kinematics handling

// here by adding other case statementsfor other

// Kinematics_Type values

default:

ErrCode = -1; // Invalid Kinematics Type Entered

break;

}

return ErrCode;

}

int InverseKinematics(int CS_Number,int Kinematics_Type, LocalData *Ldata)

{

int ErrCode = 0;

switch(Kinematics_Type)

{

case KinematicsType1:

ErrCode =InverseKinematicsSubroutine1(CS_Number,Ldata);

break;

case KinematicsType2:

ErrCode =InverseKinematicsSubroutine2(CS_Number,Ldata);

break;

// Can implement other typesof inverse kinematics handling

// here by adding other casestatements for other

// Kinematics_Type values

default:

ErrCode = -1; // Invalid Kinematics Type Entered

break;

}

return ErrCode;

}

int ForwardKinematicsSubroutine1(int CS_Number,LocalData *Ldata)

{

int ErrCode = 0;

double *R;

double *L;

double *C;

double *D;

R = GetRVarPtr(Ldata);//Ldata->L + Ldata->Lindex + Ldata->Lsize;

L = GetLVarPtr(Ldata);//Ldata->L + Ldata->Lindex;

C = GetCVarPtr(Ldata);//Ldata->L + Ldata->Lindex + MAX_MOTORS;

D = GetDVarPtr(Ldata);// Ldata->D;

//** Put forward kinematicscalculations here **//

return ErrCode;

}

int InverseKinematicsSubroutine1(int CS_Number,LocalData *Ldata)

{

int ErrCode = 0;

double *R;

double *L;

double *C;

double *D;

R = GetRVarPtr(Ldata);//Ldata->L + Ldata->Lindex + Ldata->Lsize;

L = GetLVarPtr(Ldata);//Ldata->L + Ldata->Lindex;

C = GetCVarPtr(Ldata);//Ldata->L + Ldata->Lindex + MAX_MOTORS;

D = GetDVarPtr(Ldata);// Ldata->D;

//** Put inverse kinematicscalculations here **//

return ErrCode;

}

int ForwardKinematicsSubroutine2(int CS_Number,LocalData *Ldata)

{

int ErrCode = 0;

double *R;

double *L;

double *C;

double *D;

R = GetRVarPtr(Ldata);//Ldata->L + Ldata->Lindex + Ldata->Lsize;

L = GetLVarPtr(Ldata);//Ldata->L + Ldata->Lindex;

C = GetCVarPtr(Ldata);//Ldata->L + Ldata->Lindex + MAX_MOTORS;

D = GetDVarPtr(Ldata);// Ldata->D;

//** Put forward kinematicscalculations here **//

return ErrCode;

}

int InverseKinematicsSubroutine2(int CS_Number,LocalData *Ldata)

{

int ErrCode = 0;

double *R;

double *L;

double *C;

double *D;

R = GetRVarPtr(Ldata);//Ldata->L + Ldata->Lindex + Ldata->Lsize;

L = GetLVarPtr(Ldata);//Ldata->L + Ldata->Lindex;

C = GetCVarPtr(Ldata);//Ldata->L + Ldata->Lindex + MAX_MOTORS;

D = GetDVarPtr(Ldata);// Ldata->D;

//** Put inverse kinematicscalculations here **//

return ErrCode;

}

// Add any to usrcode.c otherfunctions one might need for other

// kinematics calculations oranything else CfromScript() might need

// to call

 

这个CfromScript()函数可以从脚本运动学例程中调用,如下::

// Define the same statevalues as defined in usrcode.c

#defineForward_Kinematics_State 0

#defineInverse_Kinematics_State 1

#define KinematicsType1 1

#define KinematicsType2 2

#define CS_Number_1 1

#define CS_Number_2 2

// Define storage flags forthe error code returns

csglobal ForwardKin1ErrCode,ForwardKin2ErrCode,InvKin1ErrCode,InvKin2ErrCode;

// Forward Kinematics Buffers

openforward (1) // PutCoordinate System number inside "(cs)"

ForwardKin1ErrCode =

CfromScript(CS_Number_1,Forward_Kinematics_State,KinematicsType1,0,0,0,0);

close

openforward (2) // PutCoordinate System number inside "(cs)"

ForwardKin2ErrCode =

CfromScript(CS_Number_2,Forward_Kinematics_State,KinematicsType2,0,0,0,0);

close

// Inverse Kinematics Buffers

openinverse (1) // PutCoordinate System number inside "(cs)"

InvKin1ErrCode =

CfromScript(CS_Number_1,Inverse_Kinematics_State,KinematicsType1,0,0,0,0);

close

openinverse (2) // PutCoordinate System number inside "(cs)"

InvKin2ErrCode =

CfromScript(CS_Number_2,Inverse_Kinematics_State,KinematicsType2,0,0,0,0);

Close

 

六,CfromScript其他说明

在每个函数中,可以将C子函数与内部int ErrCode变量进行比较,然后将每个函数的错误代码传递回CfromScript()并返回到调用函数。这是用户可以跟踪CfromScript()中的错误报告的一种方法;即。从调用CfromScript()的脚本程序中,用户可以读取CfromScript()返回的错误代码,以确定发生了什么。对于每种类型的错误,使用惟一的错误代码是最好的方法。

 

七,C语言应用程序说明

多个独立的c语言“应用程序”程序可以下载到Power PMAC上,并在“通用”的Linux操作系统下执行。这些是单独的程序,而不是像上面提到的其他类型的例程那样的函数。它们不是由Power PMAC内置的调度软件调用的,而是直接在Linux操作系统下执行的。

 

这些程序可以完全独立于PMAC控制任务,或者它们可以通过Power PMAC共享内存结构与这些控制任务交互。

 

每个程序都必须使用include-RtGpShm进行编译。指令使用结构定义访问预先定义的头文件。由于这些是独立的程序,因此它们必须显式地声明变量来访问结构,如上所述。