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