Update MotionServer.c

Fix for multi-group support from correction made by Ted Miller on the MotoRos2 PR#157 Port FSU speed limit handling from MotoROS1

motoman_driver/MotoPlus/MotionServer.c

@@ -1698,8 +1698,6 @@ void Ros_MotionServer_IncMoveLoopStart(Controller* controller) //<-- IP_CLK prio
 	BOOL isMissingPulse;											// Flag that there are pulses send in last cycle that are missing from the command (pulses were not processed)  
 	BOOL hasUnprocessedData;										// Flag that at least one axis (any group) still has unprecessed data. (Used to continue sending data after the queue is empty.)
 
-	ctrlGrpData.sCtrlGrp = 0;
-
 	memset(newPulseInc, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM * MAX_CONTROLLABLE_GROUPS);
 	memset(toProcessPulses, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM * MAX_CONTROLLABLE_GROUPS);
 	memset(processedPulses, 0x00, sizeof(LONG) * MP_GRP_AXES_NUM);
@@ -1721,6 +1719,7 @@ void Ros_MotionServer_IncMoveLoopStart(Controller* controller) //<-- IP_CLK prio
 	{
 		moveData.ctrl_grp |= (0x01 << i); 
 		moveData.grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(controller->ctrlGroups[i]);
+		ctrlGrpData.sCtrlGrp = controller->ctrlGroups[i]->groupId;
 		mpGetPulsePos(&ctrlGrpData, &prevPulsePosData[i]);
 	}
 
@@ -1854,6 +1853,7 @@ void Ros_MotionServer_IncMoveLoopStart(Controller* controller) //<-- IP_CLK prio
 				// Get the current controller command position and substract the previous command position
 				// and check if it matches the amount if increment sent last cycle.  If it doesn't then
 				// some pulses are missing and the amount of unprocessed pulses needs to be added to this cycle.
+				moveData.grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(controller->ctrlGroups[i]);
 				mpGetPulsePos(&ctrlGrpData, &pulsePosData);
 				isMissingPulse = FALSE;
 				for (axis = 0; axis < MP_GRP_AXES_NUM; axis++)
@@ -2066,6 +2066,7 @@ void Ros_MotionServer_IncMoveLoopStart(Controller* controller) //<-- IP_CLK prio
 			hasUnprocessedData = FALSE;
 			for (i = 0; i < controller->numGroup; i++)
 			{
+				moveData.grp_pos_info[i].pos_tag.data[0] = Ros_CtrlGroup_GetAxisConfig(controller->ctrlGroups[i]);
 				mpGetPulsePos(&ctrlGrpData, &prevPulsePosData[i]);
 			}
 		}