83 MP_APPINFO_SEND_DATA appInfoSendData;
84 MP_STD_RSP_DATA stdResponseData;
86 sprintf(appInfoSendData.AppName,
"MotoROS");
89 sprintf(appInfoSendData.Comment,
"Motoman ROS-I driver");
91 mpApplicationInfoNotify(&appInfoSendData, &stdResponseData);
106 printf(
"Initializing controller\r\n");
155 printf(
"controller->numGroup = %d\n", controller->
numGroup);
169 for(grpNo=0; grpNo < MP_GRP_NUM; grpNo++)
171 if(grpNo < controller->numGroup)
190 printf(
"controller->numRobot = %d\n", controller->
numRobot);
216 controller->bSkillMotionReady[0] =
FALSE;
217 controller->bSkillMotionReady[1] =
FALSE;
228 printf(
"Failure to initialize controller\r\n");
242 puts(
"Waiting for robot alarms to clear...");
257 if((groupNo >= 0) && (groupNo < controller->numGroup))
261 printf(
"ERROR: Attempt to access invalid Group No. (%d) \r\n", groupNo);
275 struct sockaddr_in serverSockAddr;
279 sd = mpSocket(AF_INET, SOCK_STREAM, 0);
284 memset(&serverSockAddr, 0,
sizeof(
struct sockaddr_in));
285 serverSockAddr.sin_family = AF_INET;
286 serverSockAddr.sin_addr.s_addr = INADDR_ANY;
287 serverSockAddr.sin_port = mpHtons(tcpPort);
290 ret = mpBind(sd, (
struct sockaddr *)&serverSockAddr,
sizeof(
struct sockaddr_in));
292 goto closeSockHandle;
295 ret = mpListen(sd, SOMAXCONN);
297 goto closeSockHandle;
302 printf(
"Error in Ros_Controller_OpenSocket\r\n");
322 struct sockaddr_in clientSockAddr;
330 printf(
"Controller connection server running\r\n");
334 if(sdMotionServer < 0)
335 goto closeSockHandle;
338 if(sdStateServer < 0)
339 goto closeSockHandle;
343 goto closeSockHandle;
345 sdMax =
max(sdMotionServer, sdStateServer);
346 sdMax =
max(sdMax, sdIoServer);
351 FD_SET(sdMotionServer, &fds);
352 FD_SET(sdStateServer, &fds);
353 FD_SET(sdIoServer, &fds);
355 if(mpSelect(sdMax+1, &fds, NULL, NULL, NULL) > 0)
357 memset(&clientSockAddr, 0,
sizeof(clientSockAddr));
358 sizeofSockAddr =
sizeof(clientSockAddr);
361 if(FD_ISSET(sdMotionServer, &fds))
363 sdAccepted = mpAccept(sdMotionServer, (
struct sockaddr *)&clientSockAddr, &sizeofSockAddr);
370 printf(
"Failed to set TCP_NODELAY.\r\n");
377 if(FD_ISSET(sdStateServer, &fds))
379 sdAccepted = mpAccept(sdStateServer, (
struct sockaddr *)&clientSockAddr, &sizeofSockAddr);
386 printf(
"Failed to set TCP_NODELAY.\r\n");
393 if(FD_ISSET(sdIoServer, &fds))
395 sdAccepted = mpAccept(sdIoServer, (
struct sockaddr *)&clientSockAddr, &sizeofSockAddr);
402 printf(
"Failed to set TCP_NODELAY.\r\n");
411 printf(
"Error!?... Connection Server is aborting. Reboot the controller.\r\n");
415 if(sdMotionServer >= 0)
416 mpClose(sdMotionServer);
417 if(sdStateServer >= 0)
418 mpClose(sdStateServer);
449 #if (YRC1000||YRC1000u) 450 controller->
ioStatusAddr[IO_ROBOTSTATUS_PFL_STOP].ulAddr = 81702;
451 controller->
ioStatusAddr[IO_ROBOTSTATUS_PFL_ESCAPE].ulAddr = 81703;
452 controller->
ioStatusAddr[IO_ROBOTSTATUS_PFL_AVOIDING].ulAddr = 15120;
453 controller->
ioStatusAddr[IO_ROBOTSTATUS_PFL_AVOID_JOINT].ulAddr = 15124;
454 controller->
ioStatusAddr[IO_ROBOTSTATUS_PFL_AVOID_TRANS].ulAddr = 15125;
524 #ifndef DUMMY_SERVO_MODE 534 bMotionReady = bMotionReady && controller->bSkillMotionReady[0];
536 bMotionReady = bMotionReady && controller->bSkillMotionReady[0] && controller->bSkillMotionReady[1];
545 #if (YRC1000||YRC1000u) 548 ( controller->
ioStatus[IO_ROBOTSTATUS_PFL_AVOIDING]
549 && (controller->
ioStatus[IO_ROBOTSTATUS_PFL_AVOID_JOINT] || controller->
ioStatus[IO_ROBOTSTATUS_PFL_AVOID_TRANS])) != 0)
576 #ifndef DUMMY_SERVO_MODE 613 long fbPulsePos[MAX_PULSE_AXES];
614 long cmdPulsePos[MAX_PULSE_AXES];
620 if (bDataInQ ==
TRUE)
622 else if (bDataInQ ==
ERROR)
627 for (groupNo = 0; groupNo < controller->
numGroup; groupNo++)
639 for (i = 0; i < MP_GRP_AXES_NUM; i += 1)
659 memset(sendMsg, 0x00,
sizeof(
SimpleMsg));
689 return (mpReadIO(controller->
ioStatusAddr, ioStatus, IO_ROBOTSTATUS_MAX) == 0);
699 BOOL prevReadyStatus;
708 if(controller->
ioStatus[i] != ioStatus[i])
712 controller->
ioStatus[i] = ioStatus[i];
740 #if (YRC1000||YRC1000u) 741 case IO_ROBOTSTATUS_PFL_STOP:
742 case IO_ROBOTSTATUS_PFL_ESCAPE:
743 case IO_ROBOTSTATUS_PFL_AVOIDING:
759 printf(
"Robot job is ready for ROS commands.\r\n");
781 ioInfo.ulAddr = signal;
782 ret = mpReadIO(&ioInfo, &ioState, 1);
784 printf(
"mpReadIO failure (%d)\r\n", ret);
786 return (ioState != 0);
799 ioData.ulAddr = signal;
800 ioData.ulValue = status;
801 ret = mpWriteIO(&ioData, 1);
803 printf(
"mpWriteIO failure (%d)\r\n", ret);
812 MP_ALARM_CODE_RSP_DATA alarmData;
813 memset(&alarmData, 0x00,
sizeof(alarmData));
814 if(mpGetAlarmCode(&alarmData) == 0)
816 if(alarmData.usAlarmNum > 0)
817 return(alarmData.AlarmData.usAlarmNo[0]);
832 case 0x2010: memcpy(errMsg,
"Robot is in operation", errMsgSize);
break;
833 case 0x2030: memcpy(errMsg,
"In HOLD status (PP)", errMsgSize);
break;
834 case 0x2040: memcpy(errMsg,
"In HOLD status (External)", errMsgSize);
break;
835 case 0x2050: memcpy(errMsg,
"In HOLD status (Command)", errMsgSize);
break;
836 case 0x2060: memcpy(errMsg,
"In ERROR/ALARM status", errMsgSize);
break;
837 case 0x2070: memcpy(errMsg,
"In SERVO OFF status", errMsgSize);
break;
838 case 0x2080: memcpy(errMsg,
"Wrong operation mode", errMsgSize);
break;
839 case 0x3040: memcpy(errMsg,
"The home position is not registered", errMsgSize);
break;
840 case 0x3050: memcpy(errMsg,
"Out of range (ABSO data", errMsgSize);
break;
841 case 0x3400: memcpy(errMsg,
"Cannot operate MASTER JOB", errMsgSize);
break;
842 case 0x3410: memcpy(errMsg,
"The JOB name is already registered in another task", errMsgSize);
break;
843 case 0x4040: memcpy(errMsg,
"Specified JOB not found", errMsgSize);
break;
844 case 0x5200: memcpy(errMsg,
"Over data range", errMsgSize);
break;
845 default: memcpy(errMsg,
"Unspecified reason", errMsgSize);
break;
852 void Ros_Controller_ListenForSkill(
Controller* controller,
int sl)
854 SYS2MP_SENS_MSG skillMsg;
857 controller->bSkillMotionReady[sl - MP_SL_ID1] =
FALSE;
858 memset(&skillMsg, 0x00,
sizeof(SYS2MP_SENS_MSG));
866 mpEndSkillCommandProcess(sl, &skillMsg);
872 apiRet = mpReceiveSkillCommand(sl, &skillMsg);
874 if (skillMsg.main_comm != MP_SKILL_COMM)
876 printf(
"Ignoring command, because it's not a SKILL command\n");
881 switch(skillMsg.sub_comm)
884 if(strcmp(skillMsg.cmd,
"ROS-START") == 0)
886 controller->bSkillMotionReady[sl - MP_SL_ID1] =
TRUE;
888 else if(strcmp(skillMsg.cmd,
"ROS-STOP") == 0)
890 controller->bSkillMotionReady[sl - MP_SL_ID1] =
FALSE;
894 printf (
"MP_SKILL_SEND(SL_ID=%d) - %s \n", sl, skillMsg.cmd);
897 printf(
"controller->bSkillMotionReady[%d] = %d\n", (sl - MP_SL_ID1), controller->bSkillMotionReady[sl - MP_SL_ID1]);
901 printf(
"Robot job is ready for ROS commands.\r\n");
906 controller->bSkillMotionReady[sl - MP_SL_ID1] =
FALSE;
916 int vsnprintf(
char *
s,
size_t sz,
const char *fmt, va_list
args)
920 res = vsprintf(tmpBuf, fmt, args);
921 tmpBuf[
sizeof(tmpBuf) - 1] = 0;
925 printf(
"Logging.. Error vsnprintf:%d max:%d, anyway:\r\n", (
int)res, (
int)sz);
926 printf(
"%s", tmpBuf);
929 strncpy(
s, tmpBuf, sz);
935 int snprintf(
char *
s,
size_t sz,
const char *fmt, ...)
942 res = vsnprintf(tmpBuf, sz, fmt, va);
945 strncpy(
s, tmpBuf, sz);
953 const int MAX_MSG_LEN = 32;
954 char msg[MAX_MSG_LEN];
955 char subMsg[MAX_MSG_LEN];
960 memset(msg, 0x00, MAX_MSG_LEN);
961 memset(subMsg, 0x00, MAX_MSG_LEN);
963 va_start(va, msgFmtIfFalse);
964 vsnprintf(subMsg, MAX_MSG_LEN, msgFmtIfFalse, va);
967 snprintf(msg, MAX_MSG_LEN,
"MotoROS:%s", subMsg);
972 mpSetAlarm(8000, msg, subCodeIfFalse);
985 const int MAX_MSG_LEN = 128;
986 char msg[MAX_MSG_LEN];
989 memset(msg, 0x00, MAX_MSG_LEN);
991 va_start(va, msgFormat);
992 vsnprintf(msg, MAX_MSG_LEN, msgFormat, va);
1001 mpTaskDelay(milliseconds / mpGetRtc());
void Ros_Controller_SetIOState(ULONG signal, BOOL status)
int tidStateSendState[MAX_STATE_CONNECTIONS]
#define IO_FEEDBACK_CONNECTSERVERRUNNING
#define IO_FEEDBACK_WAITING_MP_INCMOVE
BOOL Ros_Controller_Init(Controller *controller)
int sdIoConnections[MAX_IO_CONNECTIONS]
void Ros_Controller_ErrNo_ToString(int errNo, char errMsg[ERROR_MSG_MAX_SIZE], int errMsgSize)
AXIS_TYPE type[MAX_PULSE_AXES]
BOOL Ros_Controller_IsHold(Controller *controller)
#define MAX_CONTROLLABLE_GROUPS
BOOL Ros_CtrlGroup_GetPulsePosCmd(CtrlGroup *ctrlGroup, long pulsePos[MAX_PULSE_AXES])
#define IO_FEEDBACK_FAILURE
BOOL Ros_Controller_IsOperating(Controller *controller)
BOOL Ros_Controller_IsServoOn(Controller *controller)
struct _SmHeader SmHeader
void reportVersionInfoToController()
#define IO_FEEDBACK_RESERVED_6
BOOL Ros_Controller_IsPlay(Controller *controller)
void Ros_Controller_ConnectionServer_Start(Controller *controller)
#define APPLICATION_VERSION
#define START_MAX_PULSE_DEVIATION
void Ros_IoServer_StartNewConnection(Controller *controller, int sd)
#define IO_FEEDBACK_RESERVED_5
CtrlGroup * ctrlGroups[MP_GRP_NUM]
#define ERROR_MSG_MAX_SIZE
BOOL Ros_CtrlGroup_GetFBPulsePos(CtrlGroup *ctrlGroup, long pulsePos[MAX_PULSE_AXES])
#define IO_FEEDBACK_RESERVED_1
long prevPulsePos[MAX_PULSE_AXES]
#define MAX_STATE_CONNECTIONS
void Db_Print(char *msgFormat,...)
#define IO_FEEDBACK_RESERVED_4
BOOL Ros_Controller_IsTeach(Controller *controller)
int sdMotionConnections[MAX_MOTION_CONNECTIONS]
void Ros_Controller_StatusInit(Controller *controller)
#define IO_FEEDBACK_MOTIONSERVERCONNECTED
SmBodyRobotStatus robotStatus
#define IO_FEEDBACK_RESERVED_8
int tidMotionConnections[MAX_MOTION_CONNECTIONS]
BOOL Ros_Controller_StatusRead(Controller *controller, USHORT ioStatus[IO_ROBOTSTATUS_MAX])
void Ros_Sleep(float milliseconds)
int Ros_Controller_OpenSocket(int tcpPort)
#define IO_FEEDBACK_RESERVED_3
BOOL Ros_Controller_IsEcoMode(Controller *controller)
int sdStateConnections[MAX_STATE_CONNECTIONS]
STATUS setsockopt(int s, int level, int optname, char *optval, int optlen)
int Ros_Controller_StatusToMsg(Controller *controller, SimpleMsg *sendMsg)
#define IO_FEEDBACK_MP_INCMOVE_DONE
#define IO_FEEDBACK_INITIALIZATION_DONE
void motoRosAssert(BOOL mustBeTrue, ROS_ASSERTION_CODE subCodeIfFalse, char *msgFmtIfFalse,...)
BOOL Ros_Controller_IsValidGroupNo(Controller *controller, int groupNo)
BOOL Ros_Controller_IsAlarm(Controller *controller)
#define IO_FEEDBACK_STATESERVERCONNECTED
BOOL Ros_Controller_IsRemote(Controller *controller)
BOOL Ros_Controller_IsPflActive(Controller *controller)
#define IO_FEEDBACK_RESERVED_7
#define IO_FEEDBACK_IOSERVERCONNECTED
BOOL Ros_Controller_GetIOState(ULONG signal)
BOOL Ros_Controller_StatusUpdate(Controller *controller)
BOOL Ros_MotionServer_HasDataInQueue(Controller *controller)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
#define MAX_MOTION_CONNECTIONS
int tidIoConnections[MAX_IO_CONNECTIONS]
#define IO_FEEDBACK_RESERVED_2
BOOL Ros_Controller_IsMotionReady(Controller *controller)
BOOL Ros_Controller_IsEStop(Controller *controller)
int Ros_Controller_GetNotReadySubcode(Controller *controller)
BOOL Ros_Controller_IsWaitingRos(Controller *controller)
BOOL Ros_Controller_IsError(Controller *controller)
MP_IO_INFO ioStatusAddr[IO_ROBOTSTATUS_MAX]
double max(double a, double b)
#define MAX_IO_CONNECTIONS
USHORT ioStatus[IO_ROBOTSTATUS_MAX]
void Ros_MotionServer_StartNewConnection(Controller *controller, int sd)
BOOL Ros_Controller_WaitInitReady(Controller *controller)
AXIS_MOTION_TYPE axisType
CtrlGroup * Ros_CtrlGroup_Create(int groupNo, BOOL bIsLastGrpToInit, float interpolPeriod)
int Ros_Controller_GetAlarmCode()
void Ros_StateServer_StartNewConnection(Controller *controller, int sd)
BOOL Ros_Controller_IsInMotion(Controller *controller)
BOOL Ros_MotionServer_ClearQ_All(Controller *controller)