text: ﻿// Controller.c
  //
  /*
  * Software License Agreement (BSD License) 
  *
  * Copyright (c) 2013, Yaskawa America, Inc.
  * All rights reserved.
  *
  * Redistribution and use in binary form, with or without modification,
  * is permitted provided that the following conditions are met:
  *
  *       * Redistributions in binary form must reproduce the above copyright
  *       notice, this list of conditions and the following disclaimer in the
  *       documentation and/or other materials provided with the distribution.
  *       * Neither the name of the Yaskawa America, Inc., nor the names 
  *       of its contributors may be used to endorse or promote products derived
  *       from this software without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */ 
  
  #include "MotoROS.h"
  
  extern STATUS setsockopt
      (
      int    s,                 /* target socket */
      int    level,             /* protocol level of option */
      int    optname,           /* option name */
      char * optval,            /* pointer to option value */
      int    optlen             /* option length */
      );
  
  //-----------------------
  // Function Declarations
  //-----------------------
  BOOL Ros_Controller_Init(Controller* controller);
  BOOL Ros_Controller_WaitInitReady(Controller* controller);
  BOOL Ros_Controller_IsValidGroupNo(Controller* controller, int groupNo);
  int Ros_Controller_OpenSocket(int tcpPort);
  void Ros_Controller_ConnectionServer_Start(Controller* controller);
  // Status related
  void Ros_Controller_StatusInit(Controller* controller);
  BOOL Ros_Controller_IsAlarm(Controller* controller);
  BOOL Ros_Controller_IsError(Controller* controller);
  BOOL Ros_Controller_IsPlay(Controller* controller);
  BOOL Ros_Controller_IsTeach(Controller* controller);
  BOOL Ros_Controller_IsRemote(Controller* controller);
  BOOL Ros_Controller_IsOperating(Controller* controller);
  BOOL Ros_Controller_IsHold(Controller* controller);
  BOOL Ros_Controller_IsServoOn(Controller* controller);
  BOOL Ros_Controller_IsEStop(Controller* controller);
  BOOL Ros_Controller_IsWaitingRos(Controller* controller);
  int Ros_Controller_GetNotReadySubcode(Controller* controller);
  int Ros_Controller_StatusToMsg(Controller* controller, SimpleMsg* sendMsg);
  BOOL Ros_Controller_StatusRead(Controller* controller, USHORT ioStatus[IO_ROBOTSTATUS_MAX]);
  BOOL Ros_Controller_StatusUpdate(Controller* controller);
  // Wrapper around MPFunctions
  BOOL Ros_Controller_GetIOState(ULONG signal);
  void Ros_Controller_SetIOState(ULONG signal, BOOL status);
  int Ros_Controller_GetAlarmCode();
  void Ros_Controller_ErrNo_ToString(int errNo, char errMsg[ERROR_MSG_MAX_SIZE], int errMsgSize);
  
  //-----------------------
  // Function implementation
  //-----------------------
  
  //Report version info to display on pendant (DX200 only)
  void reportVersionInfoToController()
  {
  #if DX100 || FS100
  	return;
  #else
  	MP_APPINFO_SEND_DATA appInfoSendData;
  	MP_STD_RSP_DATA stdResponseData;
  
  	sprintf(appInfoSendData.AppName, "MotoROS");
  
  	sprintf(appInfoSendData.Version, "v%s", APPLICATION_VERSION);
  	sprintf(appInfoSendData.Comment, "Motoman ROS-I driver");
  
  	mpApplicationInfoNotify(&appInfoSendData, &stdResponseData); //don't care about return value
  #endif
  }
  
  //-------------------------------------------------------------------
  // Initialize the controller structure
  // This should be done before the controller is used for anything
  //------------------------------------------------------------------- 
  BOOL Ros_Controller_Init(Controller* controller)
  {
  	int grpNo;
  	int i;
  	BOOL bInitOk;
  	STATUS status;
  	
  	printf("Initializing controller\r\n");
  
  	reportVersionInfoToController();
  
  	// Turn off all I/O signal
  	Ros_Controller_SetIOState(IO_FEEDBACK_WAITING_MP_INCMOVE, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_MP_INCMOVE_DONE, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_INITIALIZATION_DONE, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_CONNECTSERVERRUNNING, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_MOTIONSERVERCONNECTED, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_STATESERVERCONNECTED, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_IOSERVERCONNECTED, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, FALSE);
  	
  	Ros_Controller_SetIOState(IO_FEEDBACK_RESERVED_1, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_RESERVED_2, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_RESERVED_3, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_RESERVED_4, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_RESERVED_5, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_RESERVED_6, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_RESERVED_7, FALSE);
  	Ros_Controller_SetIOState(IO_FEEDBACK_RESERVED_8, FALSE);
  	
  	// Init variables and controller status
  	bInitOk = TRUE;
  	controller->bRobotJobReady = FALSE;
  	controller->bStopMotion = FALSE;
  	controller->bPFLduringRosMove = FALSE;
  	Ros_Controller_StatusInit(controller);
  	Ros_Controller_StatusRead(controller, controller->ioStatus);
  	
  	// wait for controller to be ready for reading parameter
  	Ros_Controller_WaitInitReady(controller);
  
  #if (DX100)
  	// Determine if the robot is a DX100 SDA which requires a special case for the motion API
  	status = GP_isSdaRobot(&controller->bIsDx100Sda);
  	if (status != OK)
  		bInitOk = FALSE;
  #endif
  
  	// Get the interpolation clock
  	status = GP_getInterpolationPeriod(&controller->interpolPeriod);
  	if(status!=OK)
  		bInitOk = FALSE;
  	
  	// Get the number of groups
  	controller->numGroup = GP_getNumberOfGroups();
  #ifdef DEBUG
  	printf("controller->numGroup = %d\n", controller->numGroup);
  #endif
  	if(controller->numGroup < 1)
  		bInitOk = FALSE;
  
  	if (controller->numGroup > MAX_CONTROLLABLE_GROUPS)
  	{
  		printf("!!!---Detected %d control groups.  MotoROS will only control %d.---!!!\n", controller->numGroup, MAX_CONTROLLABLE_GROUPS);
  		controller->numGroup = MAX_CONTROLLABLE_GROUPS;
  	}
  	
  	controller->numRobot = 0;
  	
  	// Check for each group
  	for(grpNo=0; grpNo < MP_GRP_NUM; grpNo++)
  	{
  		if(grpNo < controller->numGroup)
  		{
  			// Determine if specific group exists and allocate memory for it
  			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
  				controller->numRobot++;  //This counter is required for DX100 controllers with two control-groups (robot OR ext axis)
  			}
  			else
  				bInitOk = FALSE;
  		}
  		else
  			controller->ctrlGroups[grpNo] = NULL;
  	}
  
  #ifdef DEBUG
  	printf("controller->numRobot = %d\n", controller->numRobot);
  #endif
  	
  	// Initialize Thread ID and Socket to invalid value
  	controller->tidConnectionSrv = INVALID_TASK;
  
  	for (i = 0; i < MAX_IO_CONNECTIONS; i++)
  	{
  		controller->sdIoConnections[i] = INVALID_SOCKET;
  		controller->tidIoConnections[i] = INVALID_TASK;
  	}
  
  	for (i = 0; i < MAX_STATE_CONNECTIONS; i++)
  	{
  		controller->sdStateConnections[i] = INVALID_SOCKET;
  		controller->tidStateSendState[i] = INVALID_TASK;
  	}
  
  	for (i = 0; i < MAX_MOTION_CONNECTIONS; i++)
  	{
  		controller->sdMotionConnections[i] = INVALID_SOCKET;
  		controller->tidMotionConnections[i] = INVALID_TASK;
  	}
  	controller->tidIncMoveThread = INVALID_TASK;
  
  #ifdef DX100
  	controller->bSkillMotionReady[0] = FALSE;
  	controller->bSkillMotionReady[1] = FALSE;
  #endif
  
  	if(bInitOk)
  	{
  		// Turn on initialization done I/O signal
  		Ros_Controller_SetIOState(IO_FEEDBACK_INITIALIZATION_DONE, TRUE);
  	}
  	else
  	{
  		Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE);
  		printf("Failure to initialize controller\r\n");
  	}
  	
  	return bInitOk;
  }
  
  
  //-------------------------------------------------------------------
  // Wait for the controller to be ready to start initialization
  //------------------------------------------------------------------- 
  BOOL Ros_Controller_WaitInitReady(Controller* controller)
  {
  	do  //minor alarms can be delayed briefly after bootup
  	{
  		puts("Waiting for robot alarms to clear...");
  		Ros_Sleep(2500);
  		Ros_Controller_StatusRead(controller, controller->ioStatus);
  
  	}while(Ros_Controller_IsAlarm(controller));
  
  	return TRUE;
  }
  
  
  //-------------------------------------------------------------------
  // Check the number of inc_move currently in the specified queue
  //-------------------------------------------------------------------
  BOOL Ros_Controller_IsValidGroupNo(Controller* controller, int groupNo)
  {
  	if((groupNo >= 0) && (groupNo < controller->numGroup))
  		return TRUE;
  	else
  	{
  		printf("ERROR: Attempt to access invalid Group No. (%d) \r\n", groupNo);
  		return FALSE;
  	}
  }
  
  
  //-------------------------------------------------------------------
  // Open a socket to listen for incomming connection on specified port
  // return: <0  : Error
  // 		   >=0 : socket descriptor
  //-------------------------------------------------------------------
  int Ros_Controller_OpenSocket(int tcpPort)
  {
  	int sd;  // socket descriptor
  	struct sockaddr_in	serverSockAddr;
  	int ret;
  
  	// Open the socket
  	sd = mpSocket(AF_INET, SOCK_STREAM, 0);
  	if (sd < 0)
  		return -1;
  
  	// Set structure
  	memset(&serverSockAddr, 0, sizeof(struct sockaddr_in));
  	serverSockAddr.sin_family = AF_INET;
  	serverSockAddr.sin_addr.s_addr = INADDR_ANY;
  	serverSockAddr.sin_port = mpHtons(tcpPort);
  
  	//bind to network interface
  	ret = mpBind(sd, (struct sockaddr *)&serverSockAddr, sizeof(struct sockaddr_in)); 
  	if (ret < 0)
  		goto closeSockHandle;
  
  	//prepare to accept connections
  	ret = mpListen(sd, SOMAXCONN);
  	if (ret < 0)
  		goto closeSockHandle;
  
  	return sd;
  
  closeSockHandle:
  	printf("Error in Ros_Controller_OpenSocket\r\n");
  
  	if(sd >= 0)
  		mpClose(sd);
  
  	return -2;
  }
  
  
  //-----------------------------------------------------------------------
  // Main Connection Server Task that listens for new connections
  //-----------------------------------------------------------------------
  void Ros_Controller_ConnectionServer_Start(Controller* controller)
  {
  	int     sdMotionServer = INVALID_SOCKET;
  	int     sdStateServer = INVALID_SOCKET;
  	int     sdIoServer = INVALID_SOCKET;
  	int		sdMax;
  	struct  fd_set  fds;
  	int     sdAccepted = INVALID_SOCKET;
  	struct  sockaddr_in     clientSockAddr;
  	int     sizeofSockAddr;
  	int     useNoDelay = 1;
  	STATUS  s;
  
  	//Set feedback signal (Application is installed and running)
  	Ros_Controller_SetIOState(IO_FEEDBACK_CONNECTSERVERRUNNING, TRUE);
  
  	printf("Controller connection server running\r\n");
  
  	//New sockets for server listening to multiple port
  	sdMotionServer = Ros_Controller_OpenSocket(TCP_PORT_MOTION);
  	if(sdMotionServer < 0)
  		goto closeSockHandle;
  	
  	sdStateServer = Ros_Controller_OpenSocket(TCP_PORT_STATE);
  	if(sdStateServer < 0)
  		goto closeSockHandle;
  	
  	sdIoServer = Ros_Controller_OpenSocket(TCP_PORT_IO);
  	if(sdIoServer < 0)
  		goto closeSockHandle;
  
  	sdMax = max(sdMotionServer, sdStateServer);
  	sdMax = max(sdMax, sdIoServer);
  
  	FOREVER //Continue to accept multiple connections forever
  	{
  		FD_ZERO(&fds);
  		FD_SET(sdMotionServer, &fds); 
  		FD_SET(sdStateServer, &fds); 
  		FD_SET(sdIoServer, &fds); 
  		
  		if(mpSelect(sdMax+1, &fds, NULL, NULL, NULL) > 0)
  		{
  			memset(&clientSockAddr, 0, sizeof(clientSockAddr));
  			sizeofSockAddr = sizeof(clientSockAddr);
  			
  			//Check motion server
  			if(FD_ISSET(sdMotionServer, &fds))
  			{
  				sdAccepted = mpAccept(sdMotionServer, (struct sockaddr *)&clientSockAddr, &sizeofSockAddr);
  				if (sdAccepted < 0)
  					break;
  					
  				s = setsockopt(sdAccepted, IPPROTO_TCP, TCP_NODELAY, (char*)&useNoDelay, sizeof (int));
  				if( OK != s )
  				{
  					printf("Failed to set TCP_NODELAY.\r\n");
  				}
  				
  				Ros_MotionServer_StartNewConnection(controller, sdAccepted);
  			}
  			
  			//Check state server
  			if(FD_ISSET(sdStateServer, &fds))
  			{
  				sdAccepted = mpAccept(sdStateServer, (struct sockaddr *)&clientSockAddr, &sizeofSockAddr);
  				if (sdAccepted < 0)
  					break;
  					
  				s = setsockopt(sdAccepted, IPPROTO_TCP, TCP_NODELAY, (char*)&useNoDelay, sizeof (int));
  				if( OK != s )
  				{
  					printf("Failed to set TCP_NODELAY.\r\n");
  				}
  				
  				Ros_StateServer_StartNewConnection(controller, sdAccepted);
  			}
  			
  			//Check IO server
  			if(FD_ISSET(sdIoServer, &fds))
  			{
  				sdAccepted = mpAccept(sdIoServer, (struct sockaddr *)&clientSockAddr, &sizeofSockAddr);
  				if (sdAccepted < 0)
  					break;
  					
  				s = setsockopt(sdAccepted, IPPROTO_TCP, TCP_NODELAY, (char*)&useNoDelay, sizeof (int));
  				if( OK != s )
  				{
  					printf("Failed to set TCP_NODELAY.\r\n");
  				}
  				
  				Ros_IoServer_StartNewConnection(controller, sdAccepted);
  			}
  		}
  	}
  	
  closeSockHandle:
  	printf("Error!?... Connection Server is aborting.  Reboot the controller.\r\n");
  
  	if(sdIoServer >= 0)
  		mpClose(sdIoServer);
  	if(sdMotionServer >= 0)
  		mpClose(sdMotionServer);
  	if(sdStateServer >= 0)
  		mpClose(sdStateServer);
  
  	//disable feedback signal
  	Ros_Controller_SetIOState(IO_FEEDBACK_CONNECTSERVERRUNNING, FALSE);
  }
  
  
  
  /**** Controller Status functions ****/
  
  //-------------------------------------------------------------------
  // Initialize list of Specific Input to keep track of
  //-------------------------------------------------------------------
  void Ros_Controller_StatusInit(Controller* controller)
  {
  	controller->ioStatusAddr[IO_ROBOTSTATUS_ALARM_MAJOR].ulAddr = 50010;		// Alarm
  	controller->ioStatusAddr[IO_ROBOTSTATUS_ALARM_MINOR].ulAddr = 50011;		// Alarm
  	controller->ioStatusAddr[IO_ROBOTSTATUS_ALARM_SYSTEM].ulAddr = 50012;		// Alarm
  	controller->ioStatusAddr[IO_ROBOTSTATUS_ALARM_USER].ulAddr = 50013;			// Alarm
  	controller->ioStatusAddr[IO_ROBOTSTATUS_ERROR].ulAddr = 50014;				// Error
  	controller->ioStatusAddr[IO_ROBOTSTATUS_PLAY].ulAddr = 50054;				// Play
  	controller->ioStatusAddr[IO_ROBOTSTATUS_TEACH].ulAddr = 50053;				// Teach
  	controller->ioStatusAddr[IO_ROBOTSTATUS_REMOTE].ulAddr = 80011; //50056;	// Remote  // Modified E.M. 7/9/2013
  	controller->ioStatusAddr[IO_ROBOTSTATUS_OPERATING].ulAddr = 50070;			// Operating
  	controller->ioStatusAddr[IO_ROBOTSTATUS_HOLD].ulAddr = 50071;				// Hold
  	controller->ioStatusAddr[IO_ROBOTSTATUS_SERVO].ulAddr = 50073;   			// Servo ON
  	controller->ioStatusAddr[IO_ROBOTSTATUS_ESTOP_EX].ulAddr = 80025;   		// External E-Stop
  	controller->ioStatusAddr[IO_ROBOTSTATUS_ESTOP_PP].ulAddr = 80026;   		// Pendant E-Stop
  	controller->ioStatusAddr[IO_ROBOTSTATUS_ESTOP_CTRL].ulAddr = 80027;   		// Controller E-Stop
  	controller->ioStatusAddr[IO_ROBOTSTATUS_WAITING_ROS].ulAddr = IO_FEEDBACK_WAITING_MP_INCMOVE; // Job input signaling ready for external motion
  	controller->ioStatusAddr[IO_ROBOTSTATUS_INECOMODE].ulAddr = 50727;			// Energy Saving Mode
  #if (YRC1000||YRC1000u)
  	controller->ioStatusAddr[IO_ROBOTSTATUS_PFL_STOP].ulAddr = 81702;			// PFL function stopped the motion
  	controller->ioStatusAddr[IO_ROBOTSTATUS_PFL_ESCAPE].ulAddr = 81703;			// PFL function escape from clamping motion
  	controller->ioStatusAddr[IO_ROBOTSTATUS_PFL_AVOIDING].ulAddr = 15120;		// PFL function avoidance operating
  	controller->ioStatusAddr[IO_ROBOTSTATUS_PFL_AVOID_JOINT].ulAddr = 15124;	// PFL function avoidance joint enabled
  	controller->ioStatusAddr[IO_ROBOTSTATUS_PFL_AVOID_TRANS].ulAddr = 15125;	// PFL function avoidance translation enabled
  #endif
  	controller->alarmCode = 0;
  }
  
  
  BOOL Ros_Controller_IsAlarm(Controller* controller)
  {
  	return ((controller->ioStatus[IO_ROBOTSTATUS_ALARM_MAJOR]!=0) 
  		|| (controller->ioStatus[IO_ROBOTSTATUS_ALARM_MINOR]!=0)
  		|| (controller->ioStatus[IO_ROBOTSTATUS_ALARM_SYSTEM]!=0)
  		|| (controller->ioStatus[IO_ROBOTSTATUS_ALARM_USER]!=0) );
  }
  
  BOOL Ros_Controller_IsError(Controller* controller)
  {
  	return ((controller->ioStatus[IO_ROBOTSTATUS_ERROR]!=0));
  }
  
  BOOL Ros_Controller_IsPlay(Controller* controller)
  {
  	return ((controller->ioStatus[IO_ROBOTSTATUS_PLAY]!=0));
  }
  
  BOOL Ros_Controller_IsTeach(Controller* controller)
  {
  	return ((controller->ioStatus[IO_ROBOTSTATUS_TEACH]!=0));
  }
  
  BOOL Ros_Controller_IsRemote(Controller* controller)
  {
  	return ((controller->ioStatus[IO_ROBOTSTATUS_REMOTE]!=0));
  }
  
  BOOL Ros_Controller_IsOperating(Controller* controller)
  {
  	return ((controller->ioStatus[IO_ROBOTSTATUS_OPERATING]!=0));
  }
  
  BOOL Ros_Controller_IsHold(Controller* controller)
  {
  	return ((controller->ioStatus[IO_ROBOTSTATUS_HOLD]!=0));
  }
  
  BOOL Ros_Controller_IsServoOn(Controller* controller)
  {
  	return ((controller->ioStatus[IO_ROBOTSTATUS_SERVO] != 0) && (controller->ioStatus[IO_ROBOTSTATUS_INECOMODE] == 0));
  }
  
  BOOL Ros_Controller_IsEcoMode(Controller* controller)
  {
  	return (controller->ioStatus[IO_ROBOTSTATUS_INECOMODE] != 0);
  }
  
  BOOL Ros_Controller_IsEStop(Controller* controller)
  {
  	return ((controller->ioStatus[IO_ROBOTSTATUS_ESTOP_EX]==0) 
  		|| (controller->ioStatus[IO_ROBOTSTATUS_ESTOP_PP]==0)
  		|| (controller->ioStatus[IO_ROBOTSTATUS_ESTOP_CTRL]==0) );
  }
  
  BOOL Ros_Controller_IsWaitingRos(Controller* controller)
  {
  	return ((controller->ioStatus[IO_ROBOTSTATUS_WAITING_ROS]!=0));
  }
  
  BOOL Ros_Controller_IsMotionReady(Controller* controller)
  {
  	BOOL bMotionReady;
  	
  #ifndef DUMMY_SERVO_MODE	
  	bMotionReady = controller->bRobotJobReady && Ros_Controller_IsOperating(controller) && Ros_Controller_IsRemote(controller)
  		&& !Ros_Controller_IsPflActive(controller);
  #else
  	bMotionReady = controller->bRobotJobReady && Ros_Controller_IsOperating(controller);
  #endif
  
  
  #ifdef DX100
  	if(controller->numRobot < 2)
  		bMotionReady = bMotionReady && controller->bSkillMotionReady[0];
  	else
  		bMotionReady = bMotionReady && controller->bSkillMotionReady[0] && controller->bSkillMotionReady[1];
  #endif
  
  	return bMotionReady;
  }
  
  
  BOOL Ros_Controller_IsPflActive(Controller* controller)
  {
  #if (YRC1000||YRC1000u)
  	//return ((controller->ioStatus[IO_ROBOTSTATUS_PFL_STOP] | controller->ioStatus[IO_ROBOTSTATUS_PFL_ESCAPE]) != 0);
  	if (controller->bPFLduringRosMove || controller->ioStatus[IO_ROBOTSTATUS_PFL_STOP] || controller->ioStatus[IO_ROBOTSTATUS_PFL_ESCAPE] ||
  		( controller->ioStatus[IO_ROBOTSTATUS_PFL_AVOIDING]
  		&& (controller->ioStatus[IO_ROBOTSTATUS_PFL_AVOID_JOINT] || controller->ioStatus[IO_ROBOTSTATUS_PFL_AVOID_TRANS])) != 0)
  	{
  		return TRUE;
  	}
  #endif
  	return FALSE;
  }
  
  
  int Ros_Controller_GetNotReadySubcode(Controller* controller)
  {
  	// Check alarm
  	if(Ros_Controller_IsAlarm(controller))
  		return ROS_RESULT_NOT_READY_ALARM;
  	
  	// Check error
  	if(Ros_Controller_IsError(controller))
  		return ROS_RESULT_NOT_READY_ERROR;
  	
  	// Check e-stop
  	if(Ros_Controller_IsEStop(controller))
  		return ROS_RESULT_NOT_READY_ESTOP;
  
  	// Check play mode
  	if(!Ros_Controller_IsPlay(controller))
  		return ROS_RESULT_NOT_READY_NOT_PLAY;
  	
  #ifndef DUMMY_SERVO_MODE
  	// Check remote
  	if(!Ros_Controller_IsRemote(controller))
  		return ROS_RESULT_NOT_READY_NOT_REMOTE;
  	
  	// Check servo power
  	if(!Ros_Controller_IsServoOn(controller))
  		return ROS_RESULT_NOT_READY_SERVO_OFF;
  #endif
  
  	// Check hold
  	if(Ros_Controller_IsHold(controller))
  		return ROS_RESULT_NOT_READY_HOLD;
  
  	// Check PFL active
  	if (Ros_Controller_IsPflActive(controller))
  		return ROS_RESULT_NOT_READY_PFL_ACTIVE;
  
  	// Check operating
  	if(!Ros_Controller_IsOperating(controller))
  		return ROS_RESULT_NOT_READY_NOT_STARTED;
  	
  	// Check ready I/O signal (should confirm wait)
  	if(!Ros_Controller_IsWaitingRos(controller))
  		return ROS_RESULT_NOT_READY_WAITING_ROS;
  
  	// Check skill send command
  	if(!Ros_Controller_IsMotionReady(controller))
  		return ROS_RESULT_NOT_READY_SKILLSEND;
  		
  	return ROS_RESULT_NOT_READY_UNSPECIFIED;
  }
  
  BOOL Ros_Controller_IsInMotion(Controller* controller)
  {
  	int i;
  	int groupNo;
  	long fbPulsePos[MAX_PULSE_AXES];
  	long cmdPulsePos[MAX_PULSE_AXES];
  	BOOL bDataInQ;
  	CtrlGroup* ctrlGroup;
  
  	bDataInQ = Ros_MotionServer_HasDataInQueue(controller);
  
  	if (bDataInQ == TRUE)
  		return TRUE;
  	else if (bDataInQ == ERROR)
  		return ERROR;
  	else
  	{
  		//for each control group
  		for (groupNo = 0; groupNo < controller->numGroup; groupNo++)
  		{
  			//Check group number valid
  			if (!Ros_Controller_IsValidGroupNo(controller, groupNo))
  				continue;
  
  			//Check if the feeback position has caught up to the command position
  			ctrlGroup = controller->ctrlGroups[groupNo];
  
  			Ros_CtrlGroup_GetFBPulsePos(ctrlGroup, fbPulsePos);
  			Ros_CtrlGroup_GetPulsePosCmd(ctrlGroup, cmdPulsePos);
  
  			for (i = 0; i < MP_GRP_AXES_NUM; i += 1)
  			{
  				if (ctrlGroup->axisType.type[i] != AXIS_INVALID)
  				{
  					// Check if position matches current command position
  					if (abs(fbPulsePos[i] - cmdPulsePos[i]) > START_MAX_PULSE_DEVIATION)
  						return TRUE;
  				}
  			}
  		}
  	}
  
  	return FALSE;
  }
  
  // Creates a simple message of type: ROS_MSG_ROBOT_STATUS = 13
  // Simple message containing the current state of the controller
  int Ros_Controller_StatusToMsg(Controller* controller, SimpleMsg* sendMsg)
  {
  	//initialize memory
  	memset(sendMsg, 0x00, sizeof(SimpleMsg));
  	
  	// set prefix: length of message excluding the prefix
  	sendMsg->prefix.length = sizeof(SmHeader) + sizeof(SmBodyRobotStatus);
  	
  	// set header information
  	sendMsg->header.msgType = ROS_MSG_ROBOT_STATUS;
  	sendMsg->header.commType = ROS_COMM_TOPIC;
  	sendMsg->header.replyType = ROS_REPLY_INVALID;
  	
  	// set body
  	sendMsg->body.robotStatus.drives_powered = (int)(Ros_Controller_IsServoOn(controller));
  	sendMsg->body.robotStatus.e_stopped = (int)(Ros_Controller_IsEStop(controller));
  	sendMsg->body.robotStatus.error_code = controller->alarmCode;
  	sendMsg->body.robotStatus.in_error = (int)Ros_Controller_IsAlarm(controller);
  	sendMsg->body.robotStatus.in_motion = (int)(Ros_Controller_IsInMotion(controller));
  	if(Ros_Controller_IsPlay(controller))
  		sendMsg->body.robotStatus.mode = 2;
  	else
  		sendMsg->body.robotStatus.mode = 1;
  	sendMsg->body.robotStatus.motion_possible = (int)(Ros_Controller_IsMotionReady(controller));
  	
  	return(sendMsg->prefix.length + sizeof(SmPrefix));
  }
  
  //-------------------------------------------------------------------
  // Get I/O state on the controller
  //-------------------------------------------------------------------
  BOOL Ros_Controller_StatusRead(Controller* controller, USHORT ioStatus[IO_ROBOTSTATUS_MAX])
  {
  	return (mpReadIO(controller->ioStatusAddr, ioStatus, IO_ROBOTSTATUS_MAX) == 0);
  }
  
  //-------------------------------------------------------------------
  // Update I/O state on the controller
  //-------------------------------------------------------------------
  BOOL Ros_Controller_StatusUpdate(Controller* controller)
  {
  	USHORT ioStatus[IO_ROBOTSTATUS_MAX];
  	int i;
  	BOOL prevReadyStatus;
  
  	prevReadyStatus = Ros_Controller_IsMotionReady(controller);
  	
  	if(Ros_Controller_StatusRead(controller, ioStatus))
  	{
  		// Check for change of state and potentially react to the change
  		for(i=0; i<IO_ROBOTSTATUS_MAX; i++)
  		{
  			if(controller->ioStatus[i] != ioStatus[i])
  			{
  				//printf("Change of ioStatus[%d]\r\n", i);
  				
  				controller->ioStatus[i] = ioStatus[i];
  				switch(i)
  				{
  					case IO_ROBOTSTATUS_ALARM_MAJOR: // alarm
  					case IO_ROBOTSTATUS_ALARM_MINOR: // alarm
  					case IO_ROBOTSTATUS_ALARM_SYSTEM: // alarm
  					case IO_ROBOTSTATUS_ALARM_USER: // alarm
  					{
  						if(ioStatus[IO_ROBOTSTATUS_ALARM_USER] == 0)
  							controller->alarmCode = 0;
  						else
  							controller->alarmCode = Ros_Controller_GetAlarmCode();
  
  						break;
  					}
  
  					case IO_ROBOTSTATUS_WAITING_ROS: // Job input signaling ready for external motion
  					{
  						if(ioStatus[IO_ROBOTSTATUS_WAITING_ROS] == 0)  // signal turned OFF
  						{
  							// Job execution stopped take action
  							controller->bRobotJobReady = FALSE;
  							Ros_MotionServer_ClearQ_All(controller);
  						}
  						else // signal turned ON (rising edge)
  							controller->bRobotJobReady = TRUE; //job is ready (even if other factors will prevent motion)
  						break;
  					}
  #if (YRC1000||YRC1000u)
  					case IO_ROBOTSTATUS_PFL_STOP: // PFL Stop
  					case IO_ROBOTSTATUS_PFL_ESCAPE: //  PFL Escaping
  					case IO_ROBOTSTATUS_PFL_AVOIDING: // PFL Avoidance
  					{
  						if (controller->bRobotJobReady && Ros_Controller_IsPflActive(controller))
  						{
  							// Job execution stopped by PFL take action
  							controller->bPFLduringRosMove = TRUE; //force job to be restarted with new ROS_CMD_START_TRAJ_MODE command
  							Ros_MotionServer_ClearQ_All(controller);
  						}
  						break;
  					}
  #endif
  				}
  			}
  		}
  
  		if (!prevReadyStatus && Ros_Controller_IsMotionReady(controller))
  			printf("Robot job is ready for ROS commands.\r\n");
  
  		return TRUE;
  	}
  	else
  		return FALSE;
  }
  
  
  
  /**** Wrappers on MP standard function ****/
  
  //-------------------------------------------------------------------
  // Get I/O state on the controller
  //-------------------------------------------------------------------
  BOOL Ros_Controller_GetIOState(ULONG signal)
  {
  	MP_IO_INFO ioInfo;
  	USHORT ioState;
  	int ret;
  	
  	//set feedback signal
  	ioInfo.ulAddr = signal;
  	ret = mpReadIO(&ioInfo, &ioState, 1);
  	if(ret != 0)
  		printf("mpReadIO failure (%d)\r\n", ret);
  		
  	return (ioState != 0);
  }
  
  
  //-------------------------------------------------------------------
  // Set I/O state on the controller
  //-------------------------------------------------------------------
  void Ros_Controller_SetIOState(ULONG signal, BOOL status)
  {
  	MP_IO_DATA ioData;
  	int ret;
  	
  	//set feedback signal
  	ioData.ulAddr = signal;
  	ioData.ulValue = status;
  	ret = mpWriteIO(&ioData, 1);
  	if(ret != 0)
  		printf("mpWriteIO failure (%d)\r\n", ret);
  }
  
  
  //-------------------------------------------------------------------
  // Get the code of the first alarm on the controller
  //-------------------------------------------------------------------
  int Ros_Controller_GetAlarmCode()
  {
  	MP_ALARM_CODE_RSP_DATA alarmData;
  	memset(&alarmData, 0x00, sizeof(alarmData));
  	if(mpGetAlarmCode(&alarmData) == 0)
  	{
  		if(alarmData.usAlarmNum > 0)
  			return(alarmData.AlarmData.usAlarmNo[0]);
  		else
  			return 0;
  	}
  	return -1;
  }
  
  
  //-------------------------------------------------------------------
  // Convert error code to string
  //-------------------------------------------------------------------
  void Ros_Controller_ErrNo_ToString(int errNo, char errMsg[ERROR_MSG_MAX_SIZE], int errMsgSize)
  {
  	switch(errNo)
  	{
  		case 0x2010: memcpy(errMsg, "Robot is in operation", errMsgSize); break;
  		case 0x2030: memcpy(errMsg, "In HOLD status (PP)", errMsgSize); break;
  		case 0x2040: memcpy(errMsg, "In HOLD status (External)", errMsgSize); break;
  		case 0x2050: memcpy(errMsg, "In HOLD status (Command)", errMsgSize); break;
  		case 0x2060: memcpy(errMsg, "In ERROR/ALARM status", errMsgSize); break;
  		case 0x2070: memcpy(errMsg, "In SERVO OFF status", errMsgSize); break;
  		case 0x2080: memcpy(errMsg, "Wrong operation mode", errMsgSize); break;
  		case 0x3040: memcpy(errMsg, "The home position is not registered", errMsgSize); break;
  		case 0x3050: memcpy(errMsg, "Out of range (ABSO data", errMsgSize); break;
  		case 0x3400: memcpy(errMsg, "Cannot operate MASTER JOB", errMsgSize); break;
  		case 0x3410: memcpy(errMsg, "The JOB name is already registered in another task", errMsgSize); break;
  		case 0x4040: memcpy(errMsg, "Specified JOB not found", errMsgSize); break;
  		case 0x5200: memcpy(errMsg, "Over data range", errMsgSize); break;
  		default: memcpy(errMsg, "Unspecified reason", errMsgSize); break;
  	}
  }
  
  
  #ifdef DX100
  
  void Ros_Controller_ListenForSkill(Controller* controller, int sl)
  {
  	SYS2MP_SENS_MSG skillMsg;
  	STATUS apiRet;
  	
  	controller->bSkillMotionReady[sl - MP_SL_ID1] = FALSE;
  	memset(&skillMsg, 0x00, sizeof(SYS2MP_SENS_MSG));
  	
  	FOREVER
  	{
  		//SKILL complete
  		//This will cause the SKILLSND command to complete the cursor to move to the next line.
  		//Make sure all preparation is complete to move.
  		//mpEndSkillCommandProcess(sl, &skillMsg); 
  		mpEndSkillCommandProcess(sl, &skillMsg);
  		
  		Ros_Sleep(4); //sleepy time
  		
  		//Get SKILL command
  		//task will wait for a skillsnd command in INFORM
  		apiRet = mpReceiveSkillCommand(sl, &skillMsg);
  		
  		if (skillMsg.main_comm != MP_SKILL_COMM)
  		{
  			printf("Ignoring command, because it's not a SKILL command\n");
  			continue;
  		}
  		
  		//Process SKILL command
  		switch(skillMsg.sub_comm)
  		{
  		case MP_SKILL_SEND:
  			if(strcmp(skillMsg.cmd, "ROS-START") == 0)
  			{
  				controller->bSkillMotionReady[sl - MP_SL_ID1] = TRUE;
  			}
  			else if(strcmp(skillMsg.cmd, "ROS-STOP") == 0)
  			{
  				controller->bSkillMotionReady[sl - MP_SL_ID1] = FALSE;
  			}
  			else
  			{
  				printf ("MP_SKILL_SEND(SL_ID=%d) - %s \n", sl, skillMsg.cmd);
  			}
  #ifdef DEBUG
  			printf("controller->bSkillMotionReady[%d] = %d\n", (sl - MP_SL_ID1), controller->bSkillMotionReady[sl - MP_SL_ID1]);
  #endif
  
  			if(Ros_Controller_IsMotionReady(controller))
  				printf("Robot job is ready for ROS commands.\r\n");
  			break;
  			
  		case MP_SKILL_END:
  			//ABORT!!!
  			controller->bSkillMotionReady[sl - MP_SL_ID1] = FALSE;
  			break;
  		}
  	}
  }
  #endif
  
  
  #if DX100
  // VxWorks 5.5 do not have vsnprintf, use vsprintf instead...
  int vsnprintf(char *s, size_t sz, const char *fmt, va_list args)
  {
  	char tmpBuf[1024]; // Hopefully enough...
  	size_t res;
  	res = vsprintf(tmpBuf, fmt, args);
  	tmpBuf[sizeof(tmpBuf) - 1] = 0;  // be sure ending \0 is there
  	if (res >= sz)
  	{
  		// Buffer overrun...
  		printf("Logging.. Error vsnprintf:%d max:%d, anyway:\r\n", (int)res, (int)sz);
  		printf("%s", tmpBuf);
  		res = -res;
  	}
  	strncpy(s, tmpBuf, sz);
  	s[sz - 1] = 0;  // be sure ending \0 is there
  	return res;
  }
  
  // VxWorks 5.5 do not have snprintf
  int snprintf(char *s, size_t sz, const char *fmt, ...)
  {
  	size_t res;
  	char tmpBuf[1024]; // Hopefully enough...
  	va_list va;
  
  	va_start(va, fmt);
  	res = vsnprintf(tmpBuf, sz, fmt, va);
  	va_end(va);
  
  	strncpy(s, tmpBuf, sz);
  	s[sz - 1] = 0;  // be sure ending \0 is there
  	return res;
  }
  #endif
  
  void motoRosAssert(BOOL mustBeTrue, ROS_ASSERTION_CODE subCodeIfFalse, char* msgFmtIfFalse, ...)
  {
  	const int MAX_MSG_LEN = 32;
  	char msg[MAX_MSG_LEN];
  	char subMsg[MAX_MSG_LEN];
  	va_list va;
  
  	if (!mustBeTrue)
  	{
  		memset(msg, 0x00, MAX_MSG_LEN);
  		memset(subMsg, 0x00, MAX_MSG_LEN);
  
  		va_start(va, msgFmtIfFalse);
  		vsnprintf(subMsg, MAX_MSG_LEN, msgFmtIfFalse, va);
  		va_end(va);
  
  		snprintf(msg, MAX_MSG_LEN, "MotoROS:%s", subMsg); //add "MotoROS" to distinguish from other controller alarms
  
  		Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE);
  		Ros_Controller_SetIOState(IO_FEEDBACK_INITIALIZATION_DONE, FALSE);
  
  		mpSetAlarm(8000, msg, subCodeIfFalse);
  
  		while (TRUE) //forever
  		{
  			puts(msg);
  			Ros_Sleep(5000);
  		}
  	}
  }
  
  void Db_Print(char* msgFormat, ...)
  {
  #ifdef DEBUG
  	const int MAX_MSG_LEN = 128;
  	char msg[MAX_MSG_LEN];
  	va_list va;
  
  	memset(msg, 0x00, MAX_MSG_LEN);
  
  	va_start(va, msgFormat);
  	vsnprintf(msg, MAX_MSG_LEN, msgFormat, va);
  	va_end(va);
  
  	printf(msg);
  #endif
  }
  
  void Ros_Sleep(float milliseconds)
  {
  	mpTaskDelay(milliseconds / mpGetRtc()); //Tick length varies between controller models
  }
  