MotoROS v1.7.0

motoman_driver/MotoPlus/Controller.c

@@ -227,7 +227,9 @@ BOOL Ros_Controller_Init(Controller* controller)
 		if(grpNo < controller->numGroup)
 		{
 			// Determine if specific group exists and allocate memory for it
-			controller->ctrlGroups[grpNo] = Ros_CtrlGroup_Create(grpNo, controller->interpolPeriod);
+			controller->ctrlGroups[grpNo] = Ros_CtrlGroup_Create(grpNo,								//Zero based index of the group number(0 - 3)
+																(grpNo==(controller->numGroup-1)),	//TRUE if this is the final group that is being initialized. FALSE if you plan to call this function again.
+																controller->interpolPeriod);		//Value of the interpolation period (ms) for the robot controller.
 			if(controller->ctrlGroups[grpNo] != NULL)
 			{
 				Ros_CtrlGroup_GetPulsePosCmd(controller->ctrlGroups[grpNo], controller->ctrlGroups[grpNo]->prevPulsePos); // set the current commanded pulse

motoman_driver/MotoPlus/CtrlGroup.c

@@ -31,20 +31,6 @@
 
 #include "MotoROS.h"
 
-//-----------------------
-// Function Declarations
-//-----------------------
-MP_GRP_ID_TYPE Ros_CtrlGroup_FindGrpId(int groupNo);
-CtrlGroup* Ros_CtrlGroup_Create(int groupNo, float interpolPeriod);
-BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES]);
-BOOL Ros_CtrlGroup_GetFBPulsePos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES]);
-void Ros_CtrlGroup_ConvertToRosPos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES], 
-									float rosPos[MAX_PULSE_AXES]);
-void Ros_CtrlGroup_ConvertToMotoPos(CtrlGroup* ctrlGroup, float rosPos[MAX_PULSE_AXES],
-									long pulsePos[MAX_PULSE_AXES]);
-UCHAR Ros_CtrlGroup_GetAxisConfig(CtrlGroup* ctrlGroup);
-BOOL Ros_CtrlGroup_IsRobot(CtrlGroup* ctrlGroup);
-
 //-----------------------
 // Function implementation
 //-----------------------
@@ -70,7 +56,7 @@ MP_GRP_ID_TYPE Ros_CtrlGroup_FindGrpId(int groupNo)
 // Create a CtrlGroup data structure for existing group otherwise 
 // return NULL
 //-------------------------------------------------------------------
-CtrlGroup* Ros_CtrlGroup_Create(int groupNo, float interpolPeriod)
+CtrlGroup* Ros_CtrlGroup_Create(int groupNo, BOOL bIsLastGrpToInit, float interpolPeriod)
 {
 	CtrlGroup* ctrlGroup;
 	int numAxes;
@@ -124,6 +110,15 @@ CtrlGroup* Ros_CtrlGroup_Create(int groupNo, float interpolPeriod)
 		if (status != OK)
 			bInitOk = FALSE;
 
+		status = GP_getFeedbackSpeedMRegisterAddresses(groupNo, //zero based index of the control group
+														TRUE, //If the register-speed-feedback is not enabled, automatically modify the SC.PRM file to enable this feature.
+														bIsLastGrpToInit, //If activating the reg-speed-feedback feature, delay the alarm until all the groups have been processed.
+														&ctrlGroup->speedFeedbackRegisterAddress); //[OUT] Index of the M registers containing the feedback speed values.
+		if (status != OK)
+		{
+			ctrlGroup->speedFeedbackRegisterAddress.bFeedbackSpeedEnabled = FALSE;
+		}
+
 		ctrlGroup->bIsBaxisSlave = (numAxes == 5) && slaveAxis;
 
 		//adjust the axisType field to account for robots with non-contiguous axes (such as delta or palletizing which use SLU--T axes)
@@ -211,12 +206,14 @@ CtrlGroup* Ros_CtrlGroup_Create(int groupNo, float interpolPeriod)
 
 //-------------------------------------------------------------------
 // Get the commanded pulse position in pulse (in motoman joint order)
+// Used for MOTION SERVER connection for positional planning calculations.
 //-------------------------------------------------------------------
 BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES])
 {
 	LONG status = 0;
 	MP_CTRL_GRP_SEND_DATA sData;
 	MP_PULSE_POS_RSP_DATA pulse_data;
+	float rosAnglePos[MP_GRP_AXES_NUM]; //temporary storage for B axis compensation
 	int i;
 
 	memset(pulsePos, 0, MAX_PULSE_AXES*sizeof(long));  // clear result, in case of error
@@ -252,12 +249,23 @@ BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_
 	for (i=0; i<MAX_PULSE_AXES; ++i)
 		pulsePos[i] = pulse_data.lPos[i];
 
+	// For MPL80/100 robot type (SLUBT): Controller automatically moves the B-axis
+	// to maintain orientation as other axes are moved.
+	if (ctrlGroup->bIsBaxisSlave)
+	{
+		//B axis compensation works on the ROS ANGLE positions, not on MOTO PULSE positions
+		Ros_CtrlGroup_ConvertToRosPos(ctrlGroup, pulsePos, rosAnglePos);
+		rosAnglePos[3] += -rosAnglePos[1] + rosAnglePos[2];
+		Ros_CtrlGroup_ConvertToMotoPos(ctrlGroup, rosAnglePos, pulsePos);
+	}
+
 	return TRUE;
 }
 
 
 //-------------------------------------------------------------------
-// Get the corrected feedback pulse position in pulse
+// Get the corrected feedback pulse position in pulse.
+// Used exclusively for STATE SERVER connection to report position.
 //-------------------------------------------------------------------
 BOOL Ros_CtrlGroup_GetFBPulsePos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES])
 {
@@ -321,10 +329,99 @@ BOOL Ros_CtrlGroup_GetFBPulsePos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_A
 	// assign return value
 	for (i=0; i<MAX_PULSE_AXES; ++i)
 		pulsePos[i] = pulse_data.lPos[i];
+
+	//--------------------------------------------------------------------
+	//NOTE: Do NOT apply any B axis compensation here. 
+	//		This is actual feedback which is reported to the state server.
+	//--------------------------------------------------------------------
 
 	return TRUE;
 }
 
+//-------------------------------------------------------------------
+// Get the corrected feedback pulse speed in pulse for each axis.
+//-------------------------------------------------------------------
+BOOL Ros_CtrlGroup_GetFBServoSpeed(CtrlGroup* ctrlGroup, long pulseSpeed[MAX_PULSE_AXES])
+{
+	int i;
+
+#ifndef DUMMY_SERVO_MODE
+	LONG status;
+	MP_IO_INFO registerInfo[MAX_PULSE_AXES * 2]; //values are 4 bytes, which consumes 2 registers
+	USHORT registerValues[MAX_PULSE_AXES * 2];
+	UINT32 registerValuesLong[MAX_PULSE_AXES * 2];
+	double dblRegister;
+
+	if (!ctrlGroup->speedFeedbackRegisterAddress.bFeedbackSpeedEnabled)
+		return FALSE;
+
+	for (i = 0; i < MAX_PULSE_AXES; i += 1)
+	{
+		registerInfo[i * 2].ulAddr = ctrlGroup->speedFeedbackRegisterAddress.cioAddressForAxis[i][0];
+		registerInfo[(i * 2) + 1].ulAddr = ctrlGroup->speedFeedbackRegisterAddress.cioAddressForAxis[i][1];
+	}
+
+	// get raw (uncorrected/unscaled) joint speeds
+	status = mpReadIO(registerInfo, registerValues, MAX_PULSE_AXES * 2);
+	if (status != OK)
+	{
+		printf("Failed to get pulse feedback speed: %u\n", status);
+		return FALSE;
+	}
+
+	for (i = 0; i < MAX_PULSE_AXES; i += 1)
+	{
+		//move to 32 bit storage
+		registerValuesLong[i * 2] = registerValues[i * 2];
+		registerValuesLong[(i * 2) + 1] = registerValues[(i * 2) + 1];
+
+		//combine both registers into single 4 byte value (0.0001 deg/sec or 1 um/sec)
+		dblRegister = (registerValuesLong[(i * 2) + 1] << 16) | registerValuesLong[i * 2];
+
+		//convert to pulse/sec
+		if (ctrlGroup->axisType.type[i] == AXIS_ROTATION)
+		{
+			dblRegister /= 1.0E4; //deg/sec
+			dblRegister *= RAD_PER_DEGREE; //rad/sec
+			dblRegister *= ctrlGroup->pulseToRad.PtoR[i]; //pulse/sec
+		}
+		else if (ctrlGroup->axisType.type[i] == AXIS_LINEAR)
+		{
+			dblRegister /= 1.0E6; //m/sec
+			dblRegister *= ctrlGroup->pulseToMeter.PtoM[i]; //pulse/sec
+		}
+
+		pulseSpeed[i] = (long)dblRegister;
+	}
+
+	// Apply correction to account for cross-axis coupling.
+	// Note: This is only required for feedback.
+	// Controller handles this correction internally when 
+	// dealing with command positon.
+	for (i = 0; i<MAX_PULSE_AXES; ++i)
+	{
+		FB_AXIS_CORRECTION *corr = &ctrlGroup->correctionData.correction[i];
+		if (corr->bValid)
+		{
+			int src_axis = corr->ulSourceAxis;
+			int dest_axis = corr->ulCorrectionAxis;
+			pulseSpeed[dest_axis] -= (int)(pulseSpeed[src_axis] * corr->fCorrectionRatio);
+		}
+	}
+#else
+	MP_CTRL_GRP_SEND_DATA sData;
+	MP_SERVO_SPEED_RSP_DATA pulse_data;
+
+	mpGetServoSpeed(&sData, &pulse_data);
+
+	// assign return value
+	for (i = 0; i<MAX_PULSE_AXES; ++i)
+		pulseSpeed[i] = pulse_data.lSpeed[i];
+#endif
+
+	return TRUE;
+}
+
 //-------------------------------------------------------------------
 // Retrieves the absolute value (Nm) of the maximum current servo torque.
 //-------------------------------------------------------------------

motoman_driver/MotoPlus/CtrlGroup.h

@@ -43,7 +43,9 @@
 
 #define	Q_OFFSET_IDX( a, b, c )	(((a)+(b)) >= (c) ) ? ((a)+(b)-(c)) \
 				: ( (((a)+(b)) < 0 ) ? ((a)+(b)+(c)) : ((a)+(b)) )
-
+
+#define RAD_PER_DEGREE (0.0174533)
+
 typedef struct
 {
 	LONG time;
@@ -61,8 +63,6 @@ typedef struct
 	Incremental_data data[Q_SIZE];
 } Incremental_q;
 
-
-
 // jointMotionData values are in radian and joint order in sequential order 
 typedef struct
 {
@@ -100,22 +100,29 @@ typedef struct
 	AXIS_MOTION_TYPE axisType;					// Indicates whether axis is rotary or linear
 
 	BOOL bIsBaxisSlave;							// Indicates the B axis will automatically move to maintain orientation as other axes are moved
+
+	JOINT_FEEDBACK_SPEED_ADDRESSES speedFeedbackRegisterAddress; //CIO address for the registers containing feedback speed
 } CtrlGroup;
 
 
 //---------------------------------
 // External Functions Declaration
 //---------------------------------
 
-extern CtrlGroup* Ros_CtrlGroup_Create(int groupNo, float interpolPeriod);
+//Initialize specific control group. This should be called for each group connected to the robot
+//controller in numerical order.
+//	int groupNo: Zero based index of the group number (0-3)
+//	BOOL bIsLastGrpToInit: TRUE if this is the final group that is being initialized. FALSE if you plan to call this function again.
+//	float interpolPeriod: Value of the interpolation period (ms) for the robot controller.
+extern CtrlGroup* Ros_CtrlGroup_Create(int groupNo, BOOL bIsLastGrpToInit, float interpolPeriod);
 
 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]);
 
 extern BOOL Ros_CtrlGroup_GetTorque(CtrlGroup* ctrlGroup, double torqueValues[MAX_PULSE_AXES]);
-extern void Ros_CtrlGroup_ConvertToRosPos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES], float rosPos[MAX_PULSE_AXES]);
 
+extern void Ros_CtrlGroup_ConvertToRosPos(CtrlGroup* ctrlGroup, long pulsePos[MAX_PULSE_AXES], float rosPos[MAX_PULSE_AXES]);
 extern void Ros_CtrlGroup_ConvertToMotoPos(CtrlGroup* ctrlGroup, float radPos[MAX_PULSE_AXES], long pulsePos[MAX_PULSE_AXES]);
 
 extern UCHAR Ros_CtrlGroup_GetAxisConfig(CtrlGroup* ctrlGroup);

motoman_driver/MotoPlus/MotionServer.c

@@ -1,13 +1,5 @@
 // MotionServer.c
 //
-// History:
-// 05/22/2013: Original release v.1.0.0
-// 06/05/2013: Fix for multi-arm control to prevent return -3 (Invalid group) 
-//			   when calling function mpExRcsIncrementMove.
-// 06/12/2013: Release v.1.0.1
-// June 2014:	Release v1.2.0
-//				Add support for multiple control groups.
-//				Add support for DX200 controller.
 /*
 * Software License Agreement (BSD License) 
 *
@@ -486,6 +478,7 @@ int Ros_MotionServer_JointTrajPtFullExProcess(Controller* controller, SimpleMsg*
 	SmBodyJointTrajPtFullEx* msgBody;	
 	CtrlGroup* ctrlGroup;
 	int ret, i;
+	FlagsValidFields validationFlags;
 
 	msgBody = &receiveMsg->body.jointTrajDataEx;
 
@@ -521,7 +514,8 @@ int Ros_MotionServer_JointTrajPtFullExProcess(Controller* controller, SimpleMsg*
 		}
 
 		// Check that minimum information (time, position, velocity) is valid
-		if( (msgBody->jointTrajPtData[i].validFields & 0x07) != 0x07 )
+		validationFlags = Valid_Time | Valid_Position | Valid_Velocity;
+		if( (msgBody->jointTrajPtData[i].validFields & validationFlags) != validationFlags)
 		{
 			printf("ERROR: Validfields = %d\r\n", msgBody->jointTrajPtData[i].validFields);
 			Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, ROS_RESULT_INVALID_DATA_INSUFFICIENT, replyMsg, msgBody->jointTrajPtData[i].groupNo);
@@ -1042,6 +1036,7 @@ int Ros_MotionServer_JointTrajDataProcess(Controller* controller, SimpleMsg* rec
 	SmBodyJointTrajPtFull* trajData;
 	CtrlGroup* ctrlGroup;
 	int ret;
+	FlagsValidFields validationFlags;
 
 	// Check if controller is able to receive incremental move and if the incremental move thread is running
 	if(!Ros_Controller_IsMotionReady(controller))
@@ -1067,7 +1062,8 @@ int Ros_MotionServer_JointTrajDataProcess(Controller* controller, SimpleMsg* rec
 	}
 
 	// Check that minimum information (time, position, velocity) is valid
-	if( (trajData->validFields & 0x07) != 0x07 )
+	validationFlags = Valid_Time | Valid_Position | Valid_Velocity;
+	if( (trajData->validFields & validationFlags) != validationFlags)
 	{
 		printf("ERROR: Validfields = %d\r\n", trajData->validFields);
 		Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, ROS_RESULT_INVALID_DATA_INSUFFICIENT, replyMsg, receiveMsg->body.jointTrajData.groupNo);
@@ -1141,6 +1137,15 @@ int Ros_MotionServer_InitTrajPointFull(CtrlGroup* ctrlGroup, SmBodyJointTrajPtFu
 		Ros_MotionServer_ConvertToJointMotionData(jointTrajData, &ctrlGroup->jointMotionData);
 		ctrlGroup->timeLeftover_ms = 0;
 		ctrlGroup->q_time = ctrlGroup->jointMotionData.time;
+
+		// For MPL80/100 robot type (SLUBT): Controller automatically moves the B-axis
+		// to maintain orientation as other axes are moved.
+		if (ctrlGroup->bIsBaxisSlave)
+		{
+			//ROS joint order
+			ctrlGroup->jointMotionData.pos[3] += -ctrlGroup->jointMotionData.pos[1] + ctrlGroup->jointMotionData.pos[2];
+			ctrlGroup->jointMotionData.vel[3] += -ctrlGroup->jointMotionData.vel[1] + ctrlGroup->jointMotionData.vel[2];
+		}
 
 		// Convert start position to pulse format
 		Ros_CtrlGroup_ConvertToMotoPos(ctrlGroup, ctrlGroup->jointMotionData.pos, pulsePos);
@@ -1155,15 +1160,19 @@ int Ros_MotionServer_InitTrajPointFull(CtrlGroup* ctrlGroup, SmBodyJointTrajPtFu
 			// Check if position matches current command position
 			if(abs(pulsePos[i] - curPos[i]) > START_MAX_PULSE_DEVIATION)
 			{
-				printf("ERROR: Trajectory start position doesn't match current position.\r\n");
-				printf("    %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld\r\n",
+				printf("ERROR: Trajectory start position doesn't match current position (MOTO joint order).\r\n");
+				printf(" - Requested start: %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld\r\n",
 					pulsePos[0], pulsePos[1], pulsePos[2],
 					pulsePos[3], pulsePos[4], pulsePos[5],
 					pulsePos[6], pulsePos[7]);
-				printf("    %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld\r\n",
+				printf(" - Current pos: %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld\r\n",
 					curPos[0], curPos[1], curPos[2],
 					curPos[3], curPos[4], curPos[5],
 					curPos[6], curPos[7]);
+				printf(" - ctrlGroup->prevPulsePos: %ld, %ld, %ld, %ld, %ld, %ld, %ld, %ld\r\n",
+					ctrlGroup->prevPulsePos[0], ctrlGroup->prevPulsePos[1], ctrlGroup->prevPulsePos[2],
+					ctrlGroup->prevPulsePos[3], ctrlGroup->prevPulsePos[4], ctrlGroup->prevPulsePos[5],
+					ctrlGroup->prevPulsePos[6], ctrlGroup->prevPulsePos[7]);
 
 				return ROS_RESULT_INVALID_DATA_START_POS;
 			}
@@ -1174,15 +1183,7 @@ int Ros_MotionServer_InitTrajPointFull(CtrlGroup* ctrlGroup, SmBodyJointTrajPtFu
 				// excessive speed
 				return ROS_RESULT_INVALID_DATA_SPEED;
 			}
-		}
-
-		// For MPL80/100 robot type (SLUBT): Controller automatically moves the B-axis
-		// to maintain orientation as other axes are moved.
-		if (ctrlGroup->bIsBaxisSlave)
-		{
-			ctrlGroup->jointMotionData.pos[3] += -ctrlGroup->jointMotionData.pos[1] + ctrlGroup->jointMotionData.pos[2];
-			ctrlGroup->jointMotionData.vel[3] += -ctrlGroup->jointMotionData.vel[1] + ctrlGroup->jointMotionData.vel[2];
-		}
+		}		
 
 		//printf("Trajectory Start Initialized\r\n");
 		// Return success
@@ -1327,6 +1328,7 @@ void Ros_MotionServer_JointTrajDataToIncQueue(Controller* controller, int groupN
 	// to maintain orientation as other axes are moved.
 	if (ctrlGroup->bIsBaxisSlave)
 	{
+		//ROS joint order
 		endTrajData->pos[3] += -endTrajData->pos[1] + endTrajData->pos[2];
 		endTrajData->vel[3] += -endTrajData->vel[1] + endTrajData->vel[2];
 	}

motoman_driver/MotoPlus/MotoROS.h

@@ -32,7 +32,7 @@
 #ifndef MOTOROS_H
 #define MOTOROS_H
 
-#define APPLICATION_VERSION					"1.6.0"
+#define APPLICATION_VERSION					"1.7.0"
 
 #include "MotoPlus.h"
 #include "ParameterExtraction.h"

motoman_driver/MotoPlus/ParameterExtraction.h

@@ -178,6 +178,24 @@ extern STATUS 	GP_getPulseToMeter(int ctrlGrp, PULSE_TO_METER* PulseToMeter);
 /******************************************************************************/
 extern STATUS 	GP_isBaxisSlave(int ctrlGrp, BOOL* bBaxisIsSlave);
 
+/******************************************************************************/
+/* << 21 >>                                                              	  */
+/* Function name : STATUS GP_getFeedbackSpeedMRegisterAddresses()			  */
+/* Functionality : Obtains the MRegister CIO addresses that contain the 	  */
+/*				   feedback speed for each axis. Optionally enables this      */
+/*				   feature if not already enabled.						      */
+/* Parameter	 : int ctrlGrp - Robot control group (zero based index) [IN]  */
+/*				   BOOL bActivateIfNotEnabled - TRUE to enable feature [IN]   */
+/*				   BOOL bForceRebootAfterActivation - TRUE to force the user  */
+/*				   to reboot if this feature gets activated. Set to FALSE if  */
+/*				   you plan to enable for additional control groups. [IN]     */
+/*				   JOINT_FEEDBACK_SPEED_ADDRESSES* registerAddresses -		  */
+/*				   Obtains the CIO register address for the feedback data [OUT]*/
+/* Return value	 : Success = OK 											  */
+/*				 : Failure = NG												  */
+/******************************************************************************/
+extern STATUS	GP_getFeedbackSpeedMRegisterAddresses(int ctrlGrp, BOOL bActivateIfNotEnabled, BOOL bForceRebootAfterActivation, JOINT_FEEDBACK_SPEED_ADDRESSES* registerAddresses);
+
 #ifdef __cplusplus
 }
 #endif