Howto realize double pass option in forward kinematics w/ CfromScript

[sutty]

Hi!

My forward kinmatic subroutine is 1 single line calling CfromScript():

open forward (1)
ForwardKin1ErrCode = CfromScript(cfsNr,KinematicType,NrPoints,0,0,0,0);
close

 

Anyhow, I do not get axis velocities nor axis following errors when issuing &1v resp &1f, eventhough I tried to rebuild the double pass option (where CfromScript() simply calls ForwardKin())

double ForwardKin(int kinType, struct LocalData *Ldata)
{
double ErrCode = 0;
//double *R;
double *L;
double *C;
double *D;
int i;
int tmpKinVelEna;

//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;

//if (KinVelEna) callsub 100;
if (KinVelEna > 0.0) tmpKinVelEna=1; else tmpKinVelEna=0;

for (i=0; i<=tmpKinVelEna; i++)
{
       //double pass option?
	if (tmpKinVelEna == 1)
	{
		if (i == 0)
		{
			Ldata->Lindex = Ldata->Lindex + Ldata->Lsize;
		}
		else
		{
			Ldata->Lindex = Ldata->Lindex - Ldata->Lsize;
			KinAxisUsed = 0x1C0;				//X,Y,Z in use
		}
	}
	else
	{
			KinAxisUsed = 0x1C0;				//X,Y,Z in use
	}

	//N100:
	if (1>0) //pshm->Coord[1].HomeComplete)
	{
		if (kinType == 0)			//standard
		{
...
and so on

 

Can anybody tell me how to realize such double pass option the right way in a CfromScript() program?!!

 

Best regards,

Anton

[curtwilson]

Implement the double-pass logic in Script, then call the CfromScript subroutine to do the math-intensive calculations in C. It will be called twice when double-pass is needed.