Introduce Ros_mpCtrlGrpNo2GrpId(..)

A platform-specific mpCtrlGrpNo2GrpId(..) wrapper.

src/CtrlGroup.c

@@ -241,6 +241,34 @@ void Ros_CtrlGrp_Cleanup(CtrlGroup* ctrlGroup)
     mpSemDelete(ctrlGroup->inc_q.q_lock);
 }
 
+
+//-------------------------------------------------------------------
+// Search through the control group to find the GroupId that matches
+// the group number
+//-------------------------------------------------------------------
+MP_GRP_ID_TYPE Ros_mpCtrlGrpNo2GrpId(int groupNo)
+{
+#if defined (YRC1000) || defined (YRC1000u)
+    return mpCtrlGrpNo2GrpId(groupNo);
+
+#elif defined (FS100) || defined (DX200)
+    MP_GRP_ID_TYPE grp_id;
+
+    for(grp_id = MP_R1_GID; grp_id < MP_S24_GID; ++grp_id)
+    {
+        if(groupNo == mpCtrlGrpId2GrpNo(grp_id))
+            return grp_id;
+    }
+
+    return -1;
+
+#else
+#error "Ros_mpCtrlGrpNo2GrpId: unsupported platform"
+
+#endif
+}
+
+
 //-------------------------------------------------------------------
 // Get the commanded pulse position in pulse (in motoman joint order)
 // Used for MOTION SERVER connection for positional planning calculations.

src/CtrlGroup.h

@@ -111,6 +111,8 @@ typedef struct
 extern CtrlGroup* Ros_CtrlGroup_Create(int groupNo, BOOL bIsLastGrpToInit, float interpolPeriod);
 extern void Ros_CtrlGrp_Cleanup(CtrlGroup* ctrlGroup);
 
+extern MP_GRP_ID_TYPE Ros_mpCtrlGrpNo2GrpId(int groupNo);
+
 extern BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES]);
 extern BOOL Ros_CtrlGroup_GetFBPulsePos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES]);
 extern BOOL Ros_CtrlGroup_GetFBServoSpeed(CtrlGroup* ctrlGroup, long pulseSpeed[MAX_PULSE_AXES]);

src/MotoPlusExterns.h

@@ -17,6 +17,8 @@ extern int mpNICData(USHORT if_no, ULONG* ip_addr, ULONG* subnet_mask, UCHAR* ma
 
 extern size_t mpNumBytesFree(void);
 
+#if defined (YRC1000) || defined (YRC1000u)
 extern MP_GRP_ID_TYPE mpCtrlGrpNo2GrpId(int grp_no);
+#endif
 
 #endif // MOTOROS2_MOTOPLUS_EXTERNS_H