//-------------------------------------------------------------------
// Create a CtrlGroup data structure for existing group otherwise
// return NULL
//-------------------------------------------------------------------
CtrlGroup* Ros_CtrlGroup_Create(int groupNo, float interpolPeriod)
{
CtrlGroup* ctrlGroup;
int numAxes;
int i;
// Check if group is defined
numAxes = GP_getNumberOfAxes(groupNo);
if(numAxes > 0)
{
// Allocate and initialize memory
ctrlGroup = mpMalloc(sizeof(CtrlGroup));
memset(ctrlGroup, 0x00, sizeof(CtrlGroup));
// Populate values
ctrlGroup->groupNo = groupNo;
ctrlGroup->numAxes = numAxes;
ctrlGroup->groupId = Ros_CtrlGroup_FindGrpId(groupNo);
GP_getPulseToRad(groupNo, &ctrlGroup->pulseToRad);
GP_getFBPulseCorrection(groupNo, &ctrlGroup->correctionData);
GP_getMaxIncPerIpCycle(groupNo, interpolPeriod , &ctrlGroup->maxInc);
memset(&ctrlGroup->inc_q, 0x00, sizeof(Incremental_q));
ctrlGroup->inc_q.q_lock = mpSemBCreate(SEM_Q_FIFO, SEM_FULL);
#ifdef DX100
float speedCap = GP_getGovForIncMotion() / 100.0;
for(i=0; i<numAxes; i++)
ctrlGroup->maxInc.maxIncrement[i] *= speedCap;
#endif
// Calculate maximum speed in radian per second
long maxSpeedPulse[MP_GRP_AXES_NUM];
memset(maxSpeedPulse, 0x00, sizeof(maxSpeedPulse));
for(i=0; i<numAxes; i++)
maxSpeedPulse[i] = ctrlGroup->maxInc.maxIncrement[i] * 1000.0 / interpolPeriod;
Ros_CtrlGroup_ConvertToRosPos(ctrlGroup, maxSpeedPulse, ctrlGroup->maxSpeedRad);
printf("maxInc: %d, %d, %d, %d, %d, %d, %d\r\n",
ctrlGroup->maxInc.maxIncrement[0],ctrlGroup->maxInc.maxIncrement[1],ctrlGroup->maxInc.maxIncrement[2],
ctrlGroup->maxInc.maxIncrement[3],ctrlGroup->maxInc.maxIncrement[4],ctrlGroup->maxInc.maxIncrement[5],
ctrlGroup->maxInc.maxIncrement[6]);
printf("maxSpeedRad: %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f\r\n",
ctrlGroup->maxSpeedRad[0],ctrlGroup->maxSpeedRad[1],ctrlGroup->maxSpeedRad[2],
ctrlGroup->maxSpeedRad[3],ctrlGroup->maxSpeedRad[4],ctrlGroup->maxSpeedRad[5],
ctrlGroup->maxSpeedRad[6]);
ctrlGroup->tidAddToIncQueue = INVALID_TASK;
}
else
{
ctrlGroup = NULL;
}
return ctrlGroup;
}
bool Controller::getNumRobotAxes(int ctrl_grp, int* numAxes)
{
*numAxes = GP_getNumberOfAxes(ctrl_grp);
return (*numAxes >= 0);
}
