for(i=0;i<100;i++)
{
int cnt;
if(cnt == 0)
{
firstNum[0]=x;
firstNum[1]=y;
firstNum[2]=z;
firstNum[3]=Q4;
firstNum[4]=Q5;
}
if(cnt == 3)
{
midNum[0]=x;
midNum[1]=y;
midNum[2]=z;
midNum[3]=Q4;
midNum[4]=Q5;
}
if(cnt == 4)
{
nextNum[0]=x;
nextNum[1]=y;
nextNum[2]=z;
nextNum[3]=Q4;
nextNum[4]=Q5;
pthread_rwlock_wrlock(&(SendDatatoSlave.rw_lock));
SendDatatoSlave.EndPositionDelta[0]=nextNum[0]-firstNum[0];
SendDatatoSlave.EndPositionDelta[1]=nextNum[1]-firstNum[1];
SendDatatoSlave.EndPositionDelta[2]=nextNum[2]-firstNum[2];
SendDatatoSlave.EndPositionDelta[3]=nextNum[3]-firstNum[3];
SendDatatoSlave.EndPositionDelta[4]=nextNum[4]-firstNum[4];
SendDatatoSlave.EndPositionDelta[5]=0;
SendDatatoSlave.EndVelocityDelta[0]=(nextNum[0]-midNum[0])/T;
SendDatatoSlave.EndVelocityDelta[1]=(nextNum[1]-midNum[1])/T;
SendDatatoSlave.EndVelocityDelta[2]=(nextNum[2]-midNum[2])/T;
SendDatatoSlave.EndVelocityDelta[3]=(nextNum[3]-midNum[3])/T;
SendDatatoSlave.EndVelocityDelta[4]=(nextNum[4]-midNum[4])/T;
SendDatatoSlave.EndVelocityDelta[5]=0;
SendDatatoSlave.EndForceRef[0]=result[0];
SendDatatoSlave.EndForceRef[1]=result[1];
SendDatatoSlave.EndForceRef[2]=result[2];
SendDatatoSlave.EndForceRef[3]=result[3];
SendDatatoSlave.EndForceRef[4]=result[4];
SendDatatoSlave.EndForceRef[5]=result[5];
data_log[0] = SendDatatoSlave.EndPositionDelta[0];
data_log[1] = SendDatatoSlave.EndPositionDelta[1];
data_log[2] = SendDatatoSlave.EndPositionDelta[2];
data_log[3] = SendDatatoSlave.EndPositionDelta[3];
data_log[4] = SendDatatoSlave.EndPositionDelta[4];
data_log[5] = SendDatatoSlave.EndPositionDelta[5];
data_log[6] = SendDatatoSlave.EndVelocityDelta[0];
data_log[7] = SendDatatoSlave.EndVelocityDelta[1];
data_log[8] = SendDatatoSlave.EndVelocityDelta[2];
data_log[9] = SendDatatoSlave.EndVelocityDelta[3];
data_log[10] = SendDatatoSlave.EndVelocityDelta[4];
data_log[11] = SendDatatoSlave.EndVelocityDelta[5];
data_log[12] = SendDatatoSlave.EndForceRef[0];
data_log[13] = SendDatatoSlave.EndForceRef[1];
data_log[14] = SendDatatoSlave.EndForceRef[2];
data_log[15] = SendDatatoSlave.EndForceRef[3];
data_log[16] = SendDatatoSlave.EndForceRef[4];
data_log[17] = SendDatatoSlave.EndForceRef[5];
pthread_rwlock_unlock(&(SendDatatoSlave.rw_lock));
cnt = 0;
}
cnt++;
}
}