Failed to set TrajectoryMode: Not Ready (5) : Unable to enable drive power (5006)

[alexander-jh]

Hello, so running motoman on a MH-5 from a FS-100 controller on Melodic 1.14.3. I've gotten roadblocked with the below error trying to enable:

$ rosservice call robot_enable
[ERROR] [1582746251.37862388]: Failed to set TrajectoryMode: Not Ready (5) : Unable to enable drive power (5006)
[ERROR] [1582746251.557802159]: Motoman robot was NOT enabled. Please re-examine and retry.

I've followed the remedy instruction as far as they'd take me. The robot_enable will turn the servo motors on, then it instantly fails.

Any feedback would be greatly appreciated.

[gavanderhoorn]

Please provide a little more information:

• version of system software on the FS100
• which tutorials / documentation did you follow to setup/configure MotoROS
• safety equipment attached to the controller (ie: e-stops, fences, other enabling devices, etc)
• any fieldbuses?
• any errors / warnings shown on the TP?

[alexander-jh]

There are no errors/warnings on the TP. The only thing other than the linux machine connected direct via ethernet is the teach pendant. Not outside safety devices either.

System: FS3.300.00A Parameter: 1.52 Language: 3.30

[gavanderhoorn]

It would be helpful if you could connect with Telnet to the controller, log in and then try calling the service.

That should provide us with a little more information.

[EricMarcil]

The error message is saying that the servos are not turning on.
You are saying that the servo DO turn on?

How long do the servo take to turn on? There is a time out of 5 sec. which is normally sufficient for the servos to turn on.

Try to turn on the servos from the pendant before starting ROS.

• Switch the pendant to PLAY mode.
• Select the INIT_ROS job
• Press the ServoOn button
• Switch the pendant to REMOTE mode. Then try starting trajectory again.

[alexander-jh]

It would be helpful if you could connect with Telnet to the controller, log in and then try calling the service.

That should provide us with a little more information.

We’re presently waiting to hear back from Yaskawa for the default VxWorks login.

The error message is saying that the servos are not turning on. You are saying that the servo DO turn on?

How long do the servo take to turn on? There is a time out of 5 sec. which is normally sufficient for the servos to turn on.

Try to turn on the servos from the pendant before starting ROS.

• Switch the pendant to PLAY mode.
• Select the INIT_ROS job
• Press the ServoOn button
• Switch the pendant to REMOTE mode. Then try starting trajectory again

Calling robot_enable turns the servos on, then there’s about a 5-10 second delay and the error comes. I will try with ROS_INIT tomorrow.

[EricMarcil]

telnet login is: MOTOMANrobot password is: MOTOMANrobot

[acbuynak]

Hello! I am working with @alexander-jh on this issue.

When I login to VxWorks using info provided by @EricMarcil, I get a continuous output of.. Failed to update controller status. Check robot parameters. After a few minutes the connection times out and stops.

Try to turn on the servos from the pendant before starting ROS.

• Switch the pendant to PLAY mode.
• Select the INIT_ROS job
• Press the ServoOn button
• Switch the pendant to REMOTE mode.
• Then try starting trajectory again.

There is no change in the error or the robot when the following trajectory command is sent. rosrun motoman_driver move_to_joint.py "[1, 0, 0, 0, 0, 0]"

I did notice that OUT#0890 came on when I send the robot_disable command.

[EricMarcil]

It indicates that the mpReadIO is failing. That is unusual...

@acbuynak @alexander-jh: Did you follow all the installation step of the installation tutorial? http://wiki.ros.org/motoman_driver/Tutorials/indigo/InstallServer

Can you e-mail me a backup of the controller parameter file (ALL.PRM) at eric.marcil71@motoman.com so that I can check parameters.

[gavanderhoorn]

This starts to sound like the controller was not (correctly) configured for use with MotoROS.

[acbuynak]

@EricMarcil I've had the email address you provided bounce back. As well as a few variations on arrangement I've tried. Is the eric.marcil71@motoman.com you gave correct?

We tried changing parameter S2C1119 from 0 to 2 to (what we think) allows a telnet connection.

When both TP & Telnet are connected to the controller. The telnet terminal outputs: Failed to update controller status. Check robot parameters.

To work around this, the TP was psychically disconnected. This allowed a telnet connection. This done, we attempted to send a trajectory command. The telnet terminal output a stream of: Waiting for Robot Alarms to clear....

[EricMarcil]

@acbuynak Sorry, I mixed up my e-mail. The correct one is: eric.marcil@motoman.com If by the TP, you mean the Teach Pendant, you need connect it unless you have a bypass plug. That's probably why you have alarms that can't be cleared.

[acbuynak]

I've fired off our parameter file to @EricMarcil for review.

In checking our controller setup further today, I realized/discovered two things.. -> I did have an issue where I could not load the actual INIT_ROS.JBI file due to this EOL error. I tried fixing it, but ended up manually typing in the job on the TP (teach pendant). When I download the JBI file and compare it to the provided job file, they are the same.

-> In this same tutorial step, I never use the IONAME.DAT file beyond saving it on my flashdrive. When comparing it to the IONAME.DAT file I downloaded from my controller just now, I noticed that that lines 482 & 483 are different.

I am missing the following... IONAME.DAT Line 482: ROS_READY,ROS_DONE,INIT_DONE,CONNECTION_SRV IONAME.DAT Line 483: MOTION_SERVER,STATE_SERVER,IO_SERVER,FAILURE

This may be the root cause. How would I upload a modified IONAME.DAT file or change it directly on the TP?

[gavanderhoorn]

The IO file just contains the names / comments for the IO elements. It's not really important.

The error message you mention, is printed here:

https://github.com/ros-industrial/motoman/blob/7de25115d7a081bf285db98972ab311c23660b65/motoman_driver/MotoPlus/mpMain.c#L120

You see this endlessly repeated, as the status task tries repeatedly to get things working. It fails, hence prints the message.

I would suggest to wait for @EricMarcil to verify your controller has the correct parameter values for use of MotoROS.

If it has been correctly configured, then we can take further steps to try and diagnose the problem.

[impet14]

I use fs100 version 3.30 with driver version 1.90. I got error print in telnet "Failed to update controller status. Check robot parameters." as well. But once I use driver version 1.70 it is fine. Thank you in advance

[gavanderhoorn]

Hm, thanks for the additional datapoint @impet14.

@EricMarcil: have you been able to take a look at the backup @alexander-jh provided?

[gavanderhoorn]

Here is the diff between MotoROS v1.7.0 and v1.9.0: https://github.com/ros-industrial/motoman/compare/4b9fd56d...f4fb6398.

Unfortunately it appears to be impossible to limit this to a certain directory only, so Github shows changes to the entire repository between those commits.

To see changes to just motoman_driver/MotoPlus, use the git CLI, navigate to the motoman clone, and run this command:

git diff 4b9fd56d..f4fb6398 motoman_driver/MotoPlus

This gives:

Diff between v1.7.0 and v1.9.0 of MotoROS (click to show)

```patch diff --git a/motoman_driver/MotoPlus/Controller.c b/motoman_driver/MotoPlus/Controller.c index 4956097..6c6b518 100644 --- a/motoman_driver/MotoPlus/Controller.c +++ b/motoman_driver/MotoPlus/Controller.c @@ -92,61 +92,6 @@ void reportVersionInfoToController() #endif } -// Verify most of the setup parameters of the robot controller. -// Please note that some parameters cannot be checked, such as -// the parameter(s) which enable this task to run. -// Returns FALSE if a critical parameter is not set such that it -// will prevent intialization. -BOOL Ros_Controller_CheckSetup() -{ - int parameterValidationCode; - - parameterValidationCode = ValidateMotoRosSetupParameters(); - switch (parameterValidationCode) - { - case MOTOROS_SETUP_OK: return TRUE; - - case MOTOROS_SETUP_RS0: - mpSetAlarm(MOTOROS_SETUPERROR_ALARMCODE, "MotoROS Cfg: Set RS000=2", parameterValidationCode); - return TRUE; - - case MOTOROS_SETUP_S2C541: - mpSetAlarm(MOTOROS_SETUPERROR_ALARMCODE, "MotoROS Cfg: Set S2C541=0", parameterValidationCode); - return TRUE; - - case MOTOROS_SETUP_S2C542: - mpSetAlarm(MOTOROS_SETUPERROR_ALARMCODE, "MotoROS Cfg: Set S2C542=0", parameterValidationCode); - return TRUE; - - case MOTOROS_SETUP_S2C1100: - mpSetAlarm(MOTOROS_SETUPERROR_ALARMCODE, "MotoROS Cfg: Set S2C1100=1", parameterValidationCode); - return FALSE; - - case MOTOROS_SETUP_S2C1103: - mpSetAlarm(MOTOROS_SETUPERROR_ALARMCODE, "MotoROS Cfg: Set S2C1103=2", parameterValidationCode); - return FALSE; - - case MOTOROS_SETUP_S2C1117: - mpSetAlarm(MOTOROS_SETUPERROR_ALARMCODE, "MotoROS Cfg: Set S2C1117=1", parameterValidationCode); - return FALSE; - - case MOTOROS_SETUP_S2C1119: - mpSetAlarm(MOTOROS_SETUPERROR_ALARMCODE, "MotoROS Cfg: Set S2C1119=0 or 2", parameterValidationCode); - return TRUE; - - case MOTOROS_SETUP_NotCompatibleWithPFL: - mpSetAlarm(MOTOROS_SETUPERROR_ALARMCODE, "MotoROS not compatible with PFL", parameterValidationCode); - return FALSE; - - //For all other error codes, please contact Yaskawa Motoman - //to have the MotoROS Runtime functionality enabled on your - //robot controller. - default: - mpSetAlarm(MOTOROS_SETUPERROR_ALARMCODE, "MotoROS: Controller cfg invalid", parameterValidationCode); - return FALSE; - } -} - //------------------------------------------------------------------- // Initialize the controller structure // This should be done before the controller is used for anything @@ -184,20 +129,19 @@ BOOL Ros_Controller_Init(Controller* controller) // Init variables and controller status bInitOk = TRUE; controller->bRobotJobReady = FALSE; - controller->bRobotJobReadyRaised = FALSE; controller->bStopMotion = FALSE; Ros_Controller_StatusInit(controller); Ros_Controller_StatusRead(controller, controller->ioStatus); // wait for controller to be ready for reading parameter Ros_Controller_WaitInitReady(controller); - - bInitOk = Ros_Controller_CheckSetup(); - if (!bInitOk) - return FALSE; //Don't allow initialization to continue - // Wait for alarms to clear, in case Ros_Controller_CheckSetup raised an alarm - 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); @@ -212,11 +156,10 @@ BOOL Ros_Controller_Init(Controller* controller) if(controller->numGroup < 1) bInitOk = FALSE; - if (controller->numGroup > MOT_MAX_GR) + if (controller->numGroup > MAX_CONTROLLABLE_GROUPS) { - mpSetAlarm(8001, "WARNING: Too many groups for ROS", 0); //force user to acknowledge ignored groups - printf("!!!---Detected %d control groups. MotoROS will only control %d.---!!!\n", controller->numGroup, MOT_MAX_GR); - controller->numGroup = MOT_MAX_GR; + printf("!!!---Detected %d control groups. MotoROS will only control %d.---!!!\n", controller->numGroup, MAX_CONTROLLABLE_GROUPS); + controller->numGroup = MAX_CONTROLLABLE_GROUPS; } controller->numRobot = 0; @@ -502,6 +445,8 @@ void Ros_Controller_StatusInit(Controller* controller) 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 + 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->alarmCode = 0; } @@ -568,18 +513,32 @@ BOOL Ros_Controller_IsWaitingRos(Controller* controller) BOOL Ros_Controller_IsMotionReady(Controller* controller) { + BOOL bMotionReady; + +#ifndef DUMMY_SERVO_MODE + bMotionReady = controller->bRobotJobReady && Ros_Controller_IsOperating(controller) && Ros_Controller_IsRemote(controller); +#else + bMotionReady = controller->bRobotJobReady && Ros_Controller_IsOperating(controller); +#endif + + #ifdef DX100 if(controller->numRobot < 2) - return (controller->bRobotJobReady && controller->bSkillMotionReady[0]); + bMotionReady = bMotionReady && controller->bSkillMotionReady[0]; else - { - return (controller->bRobotJobReady && controller->bSkillMotionReady[0] && controller->bSkillMotionReady[1]); - } -#else - return (controller->bRobotJobReady); + bMotionReady = bMotionReady && controller->bSkillMotionReady[0] && controller->bSkillMotionReady[1]; #endif + + return bMotionReady; +} + + +BOOL Ros_Controller_IsPflActive(Controller* controller) +{ + return ((controller->ioStatus[IO_ROBOTSTATUS_PFL_STOP] | controller->ioStatus[IO_ROBOTSTATUS_PFL_ESCAPE]) != 0); } + int Ros_Controller_GetNotReadySubcode(Controller* controller) { // Check alarm @@ -612,6 +571,10 @@ int Ros_Controller_GetNotReadySubcode(Controller* controller) 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; @@ -627,6 +590,50 @@ int Ros_Controller_GetNotReadySubcode(Controller* controller) 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 @@ -648,7 +655,7 @@ int Ros_Controller_StatusToMsg(Controller* controller, SimpleMsg* sendMsg) 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_MotionServer_HasDataInQueue(controller)); + sendMsg->body.robotStatus.in_motion = (int)(Ros_Controller_IsInMotion(controller)); if(Ros_Controller_IsPlay(controller)) sendMsg->body.robotStatus.mode = 2; else @@ -673,6 +680,9 @@ 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)) { @@ -691,55 +701,49 @@ BOOL Ros_Controller_StatusUpdate(Controller* controller) case IO_ROBOTSTATUS_ALARM_SYSTEM: // alarm case IO_ROBOTSTATUS_ALARM_USER: // alarm { - if(ioStatus[i] == 0) + if(ioStatus[IO_ROBOTSTATUS_ALARM_USER] == 0) controller->alarmCode = 0; else controller->alarmCode = Ros_Controller_GetAlarmCode(); + + break; } - //case IO_ROBOTSTATUS_ERROR: // error - // if(ioStatus[i] != 0) - // { - // // Take action for alarm/error handling - // } - // break; - case IO_ROBOTSTATUS_REMOTE: // remote - case IO_ROBOTSTATUS_OPERATING: // operating + case IO_ROBOTSTATUS_WAITING_ROS: // Job input signaling ready for external motion { - if(ioStatus[i] == 0) // signal turned OFF + if(ioStatus[IO_ROBOTSTATUS_WAITING_ROS] == 0) // signal turned OFF { // Job execution stopped take action controller->bRobotJobReady = FALSE; - controller->bRobotJobReadyRaised = FALSE; Ros_MotionServer_ClearQ_All(controller); } - else // signal turned ON + else // signal turned ON (rising edge) + controller->bRobotJobReady = TRUE; //job is ready (even if other factors will prevent motion) + + break; + } + case IO_ROBOTSTATUS_PFL_STOP: // PFL Stop + case IO_ROBOTSTATUS_PFL_ESCAPE: // PFL Escaping + { + if (ioStatus[IO_ROBOTSTATUS_PFL_ESCAPE] == ON) // signal turned ON { - if(i==IO_ROBOTSTATUS_WAITING_ROS) - controller->bRobotJobReadyRaised = TRUE; - -#ifndef DUMMY_SERVO_MODE - if(controller->bRobotJobReadyRaised - && (Ros_Controller_IsOperating(controller)) - && (Ros_Controller_IsRemote(controller)) ) -#else - if(controller->bRobotJobReadyRaised - && Ros_Controller_IsOperating(controller)) -#endif - { - controller->bRobotJobReady = TRUE; - if(Ros_Controller_IsMotionReady(controller)) - printf("Robot job is ready for ROS commands.\r\n"); - } + // Job execution stopped take action + controller->bRobotJobReady = FALSE; //force job to be restarted with new ROS_CMD_START_TRAJ_MODE command + Ros_MotionServer_ClearQ_All(controller); } break; } } } } + + if (!prevReadyStatus && Ros_Controller_IsMotionReady(controller)) + printf("Robot job is ready for ROS commands.\r\n"); + return TRUE; } - return FALSE; + else + return FALSE; } diff --git a/motoman_driver/MotoPlus/Controller.h b/motoman_driver/MotoPlus/Controller.h index ae8a0c8..a0769e4 100644 --- a/motoman_driver/MotoPlus/Controller.h +++ b/motoman_driver/MotoPlus/Controller.h @@ -58,6 +58,12 @@ #define MAX_MOTION_CONNECTIONS 1 #define MAX_STATE_CONNECTIONS 4 +#if (DX100) + #define MAX_CONTROLLABLE_GROUPS 3 +#else + #define MAX_CONTROLLABLE_GROUPS 4 +#endif + #define INVALID_SOCKET -1 #define INVALID_TASK -1 @@ -67,7 +73,7 @@ #define ERROR_MSG_MAX_SIZE 64 -#define START_MAX_PULSE_DEVIATION 10 +#define START_MAX_PULSE_DEVIATION 30 #define CONTROLLER_STATUS_UPDATE_PERIOD 10 @@ -92,6 +98,8 @@ typedef enum IO_ROBOTSTATUS_ESTOP_CTRL, IO_ROBOTSTATUS_WAITING_ROS, IO_ROBOTSTATUS_INECOMODE, + IO_ROBOTSTATUS_PFL_STOP, + IO_ROBOTSTATUS_PFL_ESCAPE, IO_ROBOTSTATUS_MAX } IoStatusIndex; @@ -106,8 +114,7 @@ typedef struct MP_IO_INFO ioStatusAddr[IO_ROBOTSTATUS_MAX]; // Array of Specific Input Address representing the I/O status USHORT ioStatus[IO_ROBOTSTATUS_MAX]; // Array storing the current status of the controller int alarmCode; // Alarm number currently active - BOOL bRobotJobReady; // Boolean indicating that the controller is ready for increment move - BOOL bRobotJobReadyRaised; // Indicates that the signal was raised since operating was resumed + BOOL bRobotJobReady; // Indicates the robot job is on the WAIT command (ready for motion) BOOL bStopMotion; // Flag to stop motion // Connection Server @@ -129,6 +136,7 @@ typedef struct #ifdef DX100 BOOL bSkillMotionReady[2]; // Boolean indicating that the SKILL command required for DX100 is active int RosListenForSkillID[2]; // ThreadId for listening to SkillSend command + BOOL bIsDx100Sda; // Special case to control the waist axis (axis 15) #endif } Controller; @@ -152,6 +160,7 @@ extern BOOL Ros_Controller_IsEcoMode(Controller* controller); extern BOOL Ros_Controller_IsEStop(Controller* controller); extern BOOL Ros_Controller_IsWaitingRos(Controller* controller); extern BOOL Ros_Controller_IsMotionReady(Controller* controller); +extern BOOL Ros_Controller_IsPflActive(Controller* controller); extern int Ros_Controller_GetNotReadySubcode(Controller* controller); extern int Ros_Controller_StatusToMsg(Controller* controller, SimpleMsg* sendMsg); diff --git a/motoman_driver/MotoPlus/CtrlGroup.c b/motoman_driver/MotoPlus/CtrlGroup.c index fd1237f..05e7228 100644 --- a/motoman_driver/MotoPlus/CtrlGroup.c +++ b/motoman_driver/MotoPlus/CtrlGroup.c @@ -85,6 +85,7 @@ CtrlGroup* Ros_CtrlGroup_Create(int groupNo, BOOL bIsLastGrpToInit, float interp ctrlGroup->groupNo = groupNo; ctrlGroup->numAxes = numAxes; ctrlGroup->groupId = Ros_CtrlGroup_FindGrpId(groupNo); + ctrlGroup->tool = 0; status = GP_getAxisMotionType(groupNo, &ctrlGroup->axisType); if (status != OK) @@ -346,6 +347,40 @@ BOOL Ros_CtrlGroup_GetFBServoSpeed(CtrlGroup* ctrlGroup, long pulseSpeed[MAX_PUL int i; #ifndef DUMMY_SERVO_MODE +#if (DX100 || FS100) //Use mpSvsGetVelTrqFb for older controller models + MP_GRP_AXES_T dst_vel; + LONG status; + + if (ctrlGroup->groupNo >= MAX_CONTROLLABLE_GROUPS) + return FALSE; + + memset(&dst_vel, 0x00, sizeof(MP_GRP_AXES_T)); + + status = mpSvsGetVelTrqFb(dst_vel, NULL); //units are 0.1 pulse/sec + if (status != OK) + return FALSE; + + for (i = 0; i < MAX_PULSE_AXES; i += 1) + { + pulseSpeed[i] = dst_vel[ctrlGroup->groupNo][i] * 0.1; + } + + // 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 //DX200 and newer supports the M-register analog feedback (higher precision feedback) LONG status; MP_IO_INFO registerInfo[MAX_PULSE_AXES * 2]; //values are 4 bytes, which consumes 2 registers USHORT registerValues[MAX_PULSE_AXES * 2]; @@ -393,22 +428,9 @@ BOOL Ros_CtrlGroup_GetFBServoSpeed(CtrlGroup* ctrlGroup, long pulseSpeed[MAX_PUL pulseSpeed[i] = (long)dblRegister; } +#endif - // 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; icorrectionData.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 +#else //dummy-servo mode for testing MP_CTRL_GRP_SEND_DATA sData; MP_SERVO_SPEED_RSP_DATA pulse_data; @@ -436,6 +458,8 @@ BOOL Ros_CtrlGroup_GetTorque(CtrlGroup* ctrlGroup, double torqueValues[MAX_PULSE memset(dst_trq.data, 0, sizeof(MP_TRQCTL_DATA)); dst_trq.unit = TRQ_NEWTON_METER; //request data in Nm + memset(&dst_vel, 0x00, sizeof(MP_GRP_AXES_T)); + status = mpSvsGetVelTrqFb(dst_vel, &dst_trq); if (status != OK) return FALSE; diff --git a/motoman_driver/MotoPlus/CtrlGroup.h b/motoman_driver/MotoPlus/CtrlGroup.h index f7dd184..e4fc7ab 100644 --- a/motoman_driver/MotoPlus/CtrlGroup.h +++ b/motoman_driver/MotoPlus/CtrlGroup.h @@ -87,7 +87,8 @@ typedef struct FB_PULSE_CORRECTION_DATA correctionData; // compensation for axes coupling MAX_INCREMENT_INFO maxInc; // maximum increment per interpolation cycle float maxSpeed[MP_GRP_AXES_NUM]; // maximum joint speed in radian/sec (rotational) or meter/sec (linear) - + int tool; // selected tool for the motion + Incremental_q inc_q; // incremental queue long q_time; // time to which the queue has been processed diff --git a/motoman_driver/MotoPlus/IoServer.c b/motoman_driver/MotoPlus/IoServer.c index f465821..3771522 100644 --- a/motoman_driver/MotoPlus/IoServer.c +++ b/motoman_driver/MotoPlus/IoServer.c @@ -97,6 +97,7 @@ void Ros_IoServer_StartNewConnection(Controller* controller, int sd) controller->sdIoConnections[connectionIndex] = INVALID_SOCKET; controller->tidIoConnections[connectionIndex] = INVALID_TASK; Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE); + mpSetAlarm(8004, "MOTOROS FAILED TO CREATE TASK", 7); return; } } diff --git a/motoman_driver/MotoPlus/MotionServer.c b/motoman_driver/MotoPlus/MotionServer.c index d03e214..ba5c1d1 100644 --- a/motoman_driver/MotoPlus/MotionServer.c +++ b/motoman_driver/MotoPlus/MotionServer.c @@ -53,6 +53,8 @@ int Ros_MotionServer_InitTrajPointFullEx(CtrlGroup* ctrlGroup, SmBodyJointTrajPt int Ros_MotionServer_AddTrajPointFull(CtrlGroup* ctrlGroup, SmBodyJointTrajPtFull* jointTrajData); int Ros_MotionServer_AddTrajPointFullEx(CtrlGroup* ctrlGroup, SmBodyJointTrajPtExData* jointTrajDataEx, int sequence); int Ros_MotionServer_JointTrajPtFullExProcess(Controller* controller, SimpleMsg* receiveMsg, SimpleMsg* replyMsg); +int Ros_MotionServer_GetDhParameters(Controller* controller, SimpleMsg* replyMsg); +int Ros_MotionServer_SetSelectedTool(Controller* controller, SimpleMsg* receiveMsg, SimpleMsg* replyMsg); // AddToIncQueue Task: void Ros_MotionServer_AddToIncQueueProcess(Controller* controller, int groupNo); @@ -68,13 +70,6 @@ void Ros_MotionServer_ConvertToJointMotionData(SmBodyJointTrajPtFull* jointTrajD STATUS Ros_MotionServer_DisableEcoMode(Controller* controller); void Ros_MotionServer_PrintError(USHORT err_no, char* msgPrefix); -// IO functions: -int Ros_MotionServer_ReadIOBit(SimpleMsg* receiveMsg, SimpleMsg* replyMsg); -int Ros_MotionServer_WriteIOBit(SimpleMsg* receiveMsg, SimpleMsg* replyMsg); -int Ros_MotionServer_ReadIOGroup(SimpleMsg* receiveMsg, SimpleMsg* replyMsg); -int Ros_MotionServer_WriteIOGroup(SimpleMsg* receiveMsg, SimpleMsg* replyMsg); - - //----------------------- // Function implementation //----------------------- @@ -122,6 +117,8 @@ void Ros_MotionServer_StartNewConnection(Controller* controller, int sd) mpClose(sd); controller->tidIncMoveThread = INVALID_TASK; Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE); + mpSetAlarm(8004, "MOTOROS FAILED TO CREATE TASK", 4); + return; } } @@ -142,6 +139,7 @@ void Ros_MotionServer_StartNewConnection(Controller* controller, int sd) mpClose(sd); controller->ctrlGroups[groupNo]->tidAddToIncQueue = INVALID_TASK; Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE); + mpSetAlarm(8004, "MOTOROS FAILED TO CREATE TASK", 5); return; } } @@ -169,6 +167,7 @@ void Ros_MotionServer_StartNewConnection(Controller* controller, int sd) controller->sdMotionConnections[connectionIndex] = INVALID_SOCKET; controller->tidMotionConnections[connectionIndex] = INVALID_TASK; Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE); + mpSetAlarm(8004, "MOTOROS FAILED TO CREATE TASK", 6); return; } } @@ -278,6 +277,12 @@ int Ros_MotionServer_GetExpectedByteSizeForMessageType(SimpleMsg* receiveMsg, in case ROS_MSG_MOTO_WRITE_IO_GROUP: expectedSize = minSize + sizeof(SmBodyMotoWriteIOGroup); break; + case ROS_MSG_MOTO_GET_DH_PARAMETERS: + expectedSize = minSize; //no additional data on the request + break; + case ROS_MSG_MOTO_SELECT_TOOL: + expectedSize = minSize + sizeof(SmBodySelectTool); + break; default: //invalid message type return -1; } @@ -451,6 +456,16 @@ int Ros_MotionServer_SimpleMsgProcess(Controller* controller, SimpleMsg* receive break; //----------------------- + case ROS_MSG_MOTO_GET_DH_PARAMETERS: + ret = Ros_MotionServer_GetDhParameters(controller, replyMsg); + break; + + //----------------------- + case ROS_MSG_MOTO_SELECT_TOOL: + ret = Ros_MotionServer_SetSelectedTool(controller, receiveMsg, replyMsg); + break; + + //----------------------- default: printf("Invalid message type: %d\n", receiveMsg->header.msgType); invalidSubcode = ROS_RESULT_INVALID_MSGTYPE; @@ -1070,6 +1085,15 @@ int Ros_MotionServer_JointTrajDataProcess(Controller* controller, SimpleMsg* rec return 0; } + //Due to data loss when converting from double (ROS PC) to float (Simple Message serialization), we cannot accept + //a single trjectory that lasts more than 4 hours. + if (trajData->time >= MAX_TRAJECTORY_TIME_LENGTH) + { + printf("ERROR: Trajectory time (%.4f) > MAX_TRAJECTORY_TIME_LENGTH (%.4f)\r\n", trajData->time, MAX_TRAJECTORY_TIME_LENGTH); + Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, ROS_RESULT_INVALID_DATA_TIME, replyMsg, receiveMsg->body.jointTrajData.groupNo); + return 0; + } + // Check the trajectory sequence code if(trajData->sequence == 0) // First trajectory point { @@ -1336,6 +1360,7 @@ void Ros_MotionServer_JointTrajDataToIncQueue(Controller* controller, int groupN memset(newPulsePos, 0x00, sizeof(newPulsePos)); memset(&incData, 0x00, sizeof(incData)); incData.frame = MP_INC_PULSE_DTYPE; + incData.tool = ctrlGroup->tool; // Calculate an acceleration coefficients memset(&accCoef1, 0x00, sizeof(accCoef1)); @@ -1552,7 +1577,7 @@ int Ros_MotionServer_GetQueueCnt(Controller* controller, int groupNo) } printf("ERROR: Unable to access queue count. Queue is locked up!\r\n"); - return -1; + return ERROR; } @@ -1563,11 +1588,15 @@ int Ros_MotionServer_GetQueueCnt(Controller* controller, int groupNo) BOOL Ros_MotionServer_HasDataInQueue(Controller* controller) { int groupNo; + int qCnt; for(groupNo=0; groupNonumGroup; groupNo++) { - if(Ros_MotionServer_GetQueueCnt(controller, groupNo) > 0) + qCnt = Ros_MotionServer_GetQueueCnt(controller, groupNo); + if (qCnt > 0) return TRUE; + else if (qCnt == ERROR) + return ERROR; } return FALSE; @@ -1691,7 +1720,10 @@ void Ros_MotionServer_IncMoveLoopStart(Controller* controller) //<-- IP_CLK prio #if DX100 // first robot - moveData.ctrl_grp = 1; + if (controller->bIsDx100Sda) + moveData.ctrl_grp = 1 | (1 << 2); //R1 + B1 + else + moveData.ctrl_grp = 1; //R1 only ret = mpMeiIncrementMove(MP_SL_ID1, &moveData); if(ret != 0) { @@ -1700,8 +1732,8 @@ void Ros_MotionServer_IncMoveLoopStart(Controller* controller) //<-- IP_CLK prio else printf("mpMeiIncrementMove returned: %d\r\n", ret); } - // if second robot // This is not tested but was introduce to help future development - moveData.ctrl_grp = 2; + // if second robot + moveData.ctrl_grp = 2; //R2 only if(controller->numRobot > 1) { ret = mpMeiIncrementMove(MP_SL_ID2, &moveData); @@ -1812,3 +1844,57 @@ STATUS Ros_MotionServer_DisableEcoMode(Controller* controller) else return NG; } + +int Ros_MotionServer_GetDhParameters(Controller* controller, SimpleMsg* replyMsg) +{ + int i; + STATUS apiRet = OK; + + //initialize memory + memset(replyMsg, 0x00, sizeof(SimpleMsg)); + + // set prefix: length of message excluding the prefix + replyMsg->prefix.length = sizeof(SmHeader) + sizeof(SmBodyMotoGetDhParameters); + + // set header information of the reply + replyMsg->header.msgType = ROS_MSG_MOTO_GET_DH_PARAMETERS; + replyMsg->header.commType = ROS_COMM_SERVICE_REPLY; + replyMsg->header.replyType = ROS_REPLY_SUCCESS; + + for (i = 0; i < controller->numGroup; i += 1) + { + if (controller->ctrlGroups[i] != NULL && replyMsg->header.replyType == ROS_REPLY_SUCCESS) + { + apiRet = GP_getDhParameters(i, &replyMsg->body.dhParameters.dhParameters[i]); + + if (apiRet == OK) + replyMsg->header.replyType = ROS_REPLY_SUCCESS; + else + replyMsg->header.replyType = ROS_REPLY_FAILURE; + } + } + + return apiRet; +} + + +int Ros_MotionServer_SetSelectedTool(Controller* controller, SimpleMsg* receiveMsg, SimpleMsg* replyMsg) +{ + int groupNo = receiveMsg->body.selectTool.groupNo; + int tool = receiveMsg->body.selectTool.tool; + + if (groupNo >= 0 && groupNo < controller->numRobot) + { + if (tool >= MIN_VALID_TOOL_INDEX && tool <= MAX_VALID_TOOL_INDEX) + { + controller->ctrlGroups[receiveMsg->body.selectTool.groupNo]->tool = tool; + Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_SUCCESS, 0, replyMsg, groupNo); + } + else + Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, ROS_RESULT_INVALID_DATA_TOOLNO, replyMsg, groupNo); + } + else + Ros_SimpleMsg_MotionReply(receiveMsg, ROS_RESULT_INVALID, ROS_RESULT_INVALID_GROUPNO, replyMsg, groupNo); + + return 0; +} diff --git a/motoman_driver/MotoPlus/MotionServer.h b/motoman_driver/MotoPlus/MotionServer.h index 1db367d..f50cba9 100644 --- a/motoman_driver/MotoPlus/MotionServer.h +++ b/motoman_driver/MotoPlus/MotionServer.h @@ -32,10 +32,15 @@ #ifndef MOTIONSERVER_H #define MOTIONSERVER_H -#define MOTION_STOP_TIMEOUT 20 -#define MOTION_START_TIMEOUT 5000 // in milliseconds -#define MOTION_START_CHECK_PERIOD 50 // in millisecond -#define MOTION_INIT_ROS_JOB "INIT_ROS" +#define MOTION_STOP_TIMEOUT 20 +#define MOTION_START_TIMEOUT 5000 // in milliseconds +#define MOTION_START_CHECK_PERIOD 50 // in millisecond +#define MOTION_INIT_ROS_JOB "INIT_ROS" + +#define MAX_TRAJECTORY_TIME_LENGTH 14400.0f //seconds (4 hours) + +#define MIN_VALID_TOOL_INDEX 0 +#define MAX_VALID_TOOL_INDEX 63 extern void Ros_MotionServer_StartNewConnection(Controller* controller, int sd); extern BOOL Ros_MotionServer_HasDataInQueue(Controller* controller); diff --git a/motoman_driver/MotoPlus/MotoROS.h b/motoman_driver/MotoPlus/MotoROS.h index 55bb2ac..005410e 100644 --- a/motoman_driver/MotoPlus/MotoROS.h +++ b/motoman_driver/MotoPlus/MotoROS.h @@ -32,7 +32,7 @@ #ifndef MOTOROS_H #define MOTOROS_H -#define APPLICATION_VERSION "1.7.0" +#define APPLICATION_VERSION "1.9.0" #include "MotoPlus.h" #include "ParameterExtraction.h" @@ -43,6 +43,5 @@ #include "MotionServer.h" #include "StateServer.h" #include "IoServer.h" -#include "RosSetupValidation.h" -#endif \ No newline at end of file +#endif diff --git a/motoman_driver/MotoPlus/MpRosAllControllers.sln b/motoman_driver/MotoPlus/MpRosAllControllers.sln index 37e4ded..8f2ed9b 100644 --- a/motoman_driver/MotoPlus/MpRosAllControllers.sln +++ b/motoman_driver/MotoPlus/MpRosAllControllers.sln @@ -1,7 +1,7 @@  Microsoft Visual Studio Solution File, Format Version 12.00 -# Visual Studio 14 -VisualStudioVersion = 14.0.25420.1 +# Visual Studio 15 +VisualStudioVersion = 15.0.26730.12 MinimumVisualStudioVersion = 10.0.40219.1 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "MpRosAllControllers", "MpRosAllControllers.vcxproj", "{64C31524-A3C4-49D4-AD04-955D44202226}" EndProject @@ -11,6 +11,7 @@ Global DX200|Win32 = DX200|Win32 FS100|Win32 = FS100|Win32 YRC1000|Win32 = YRC1000|Win32 + YRC1000u|Win32 = YRC1000u|Win32 EndGlobalSection GlobalSection(ProjectConfigurationPlatforms) = postSolution {64C31524-A3C4-49D4-AD04-955D44202226}.DX100|Win32.ActiveCfg = DX100|Win32 @@ -21,8 +22,13 @@ Global {64C31524-A3C4-49D4-AD04-955D44202226}.FS100|Win32.Build.0 = FS100|Win32 {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000|Win32.ActiveCfg = YRC1000|Win32 {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000|Win32.Build.0 = YRC1000|Win32 + {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000u|Win32.ActiveCfg = YRC1000u|Win32 + {64C31524-A3C4-49D4-AD04-955D44202226}.YRC1000u|Win32.Build.0 = YRC1000u|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {C6611CF4-3743-47F8-BC5B-D98E9D8FD2C0} + EndGlobalSection EndGlobal diff --git a/motoman_driver/MotoPlus/MpRosAllControllers.vcxproj b/motoman_driver/MotoPlus/MpRosAllControllers.vcxproj index 28c408c..1ae874c 100644 --- a/motoman_driver/MotoPlus/MpRosAllControllers.vcxproj +++ b/motoman_driver/MotoPlus/MpRosAllControllers.vcxproj @@ -1,5 +1,5 @@  - + DX100 @@ -13,6 +13,10 @@ FS100 Win32 + + YRC1000u + Win32 + YRC1000 Win32 @@ -21,23 +25,33 @@ {64C31524-A3C4-49D4-AD04-955D44202226} v4.5.2 + 10.0.15063.0 Makefile false + v141 Makefile false + v141 Makefile false + v141 Makefile false + v141 + + + Makefile + false + v141 @@ -54,14 +68,17 @@ + + + - mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRos$(Configuration)" -o build - MotoRos$(Configuration).out + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosDX1_" -o build + MotoRosDX1_.out $(NMakePreprocessorDefinitions);$(Configuration) $(MP_VS_Install)$(Configuration)\inc - mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRos$(Configuration)" -o rebuild - mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRos$(Configuration)" -o clean + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosDX1_" -o rebuild + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosDX1_" -o clean $(SolutionDir)output\$(Configuration)\ @@ -69,12 +86,12 @@ - mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRos$(Configuration)" -o build - MotoRos$(Configuration).out + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosFS_" -o build + MotoRosFS_.out $(NMakePreprocessorDefinitions);$(Configuration) $(MP_VS_Install)$(Configuration)\inc - mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRos$(Configuration)" -o rebuild - mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRos$(Configuration)" -o clean + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosFS_" -o rebuild + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosFS_" -o clean $(SolutionDir)output\$(Configuration)\ @@ -82,12 +99,12 @@ - mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRos$(Configuration)" -o build - MotoRos$(Configuration).out + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosDX2_" -o build + MotoRosDX2_.out $(NMakePreprocessorDefinitions);$(Configuration) $(MP_VS_Install)$(Configuration)\inc - mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRos$(Configuration)" -o rebuild - mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRos$(Configuration)" -o clean + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosDX2_" -o rebuild + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosDX2_" -o clean $(SolutionDir)output\$(Configuration)\ @@ -95,12 +112,23 @@ - mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRos$(Configuration)" -o build - MotoRos$(Configuration).out + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosYRC1_" -o build + $(MP_VS_Install)DevTools\OnlineDownload.exe $(NMakePreprocessorDefinitions);$(Configuration) $(MP_VS_Install)$(Configuration)\inc - mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRos$(Configuration)" -o rebuild - mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRos$(Configuration)" -o clean + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosYRC1_" -o rebuild + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosYRC1_" -o clean + + $(SolutionDir)output\$(Configuration)\ + + + + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosYRC1u_" -o build + $(MP_VS_Install)DevTools\OnlineDownload.exe + $(NMakePreprocessorDefinitions);$(Configuration) + $(MP_VS_Install)$(Configuration)\inc + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosYRC1u_" -o rebuild + mpbuilder.exe -c $(Configuration) -p "$(ProjectDir)\" -n "MotoRosYRC1u_" -o clean $(SolutionDir)output\$(Configuration)\ @@ -115,6 +143,11 @@ _buildLog\MotoROS$(Configuration).log + + + _buildLog\MotoROS$(Configuration).log + + _buildLog\MotoROS$(Configuration).log @@ -136,12 +169,11 @@ - - - - + + + @@ -160,7 +192,6 @@ - diff --git a/motoman_driver/MotoPlus/MpRosAllControllers.vcxproj.filters b/motoman_driver/MotoPlus/MpRosAllControllers.vcxproj.filters index df8ae3a..c27dca9 100644 --- a/motoman_driver/MotoPlus/MpRosAllControllers.vcxproj.filters +++ b/motoman_driver/MotoPlus/MpRosAllControllers.vcxproj.filters @@ -37,24 +37,12 @@ Compile Settings - - MotoPlus Libraries - - - MotoPlus Libraries - - - MotoPlus Libraries - Compile Settings Compile Settings - - MotoPlus Libraries - MotoPlus Libraries @@ -67,6 +55,15 @@ MotoPlus Libraries + + MotoPlus Libraries + + + Compile Settings + + + Compile Settings + @@ -113,9 +110,6 @@ Header Files - - Header Files - Header Files diff --git a/motoman_driver/MotoPlus/MpRosAllControllers.vcxproj.user b/motoman_driver/MotoPlus/MpRosAllControllers.vcxproj.user deleted file mode 100644 index a375ae3..0000000 --- a/motoman_driver/MotoPlus/MpRosAllControllers.vcxproj.user +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/motoman_driver/MotoPlus/MpRosDx100.mpProj b/motoman_driver/MotoPlus/MpRosDx100.mpProj deleted file mode 100644 index 63bd723..0000000 --- a/motoman_driver/MotoPlus/MpRosDx100.mpProj +++ /dev/null @@ -1,34 +0,0 @@ - - - MpRosDx100 - - Controller.c - Controller.h - CtrlGroup.c - CtrlGroup.h - MotionServer.c - MotionServer.h - mpMain.c - ParameterExtraction.h - ParameterExtraction_DX100.mpLib - SimpleMessage.c - SimpleMessage.h - StateServer.c - StateServer.h - - Controller.c - False - Executable - - 192.168.96.17 - Net_Maint_Mng - 99999999 - - - mpbuilder\bin\ccpentium.exe -mcpu=pentiumiii -march=p3 -nostdlib -fno-builtin -fno-defer-pop -W -Werror-implicit-function-declaration -Wreturn-type -Wunused -Wuninitialized -Wunreachable-code -I"~ProjectDir~" -I"~IncludeDir~" -DCPU=PENTIUM4 -DDX100 -fvolatile -O -c "~FilePath~" -o "~OutputPath~" - mpbuilder\bin\ccpentium -nostdlib -r -Wl,-X -Wl~FileList~ -o "~OutputPath~" - C:\Program Files (x86)\Yaskawa\MotoPlusIDE_DX\mpbuilder\inc - - v2.0.2 - False - \ No newline at end of file diff --git a/motoman_driver/MotoPlus/MpRosDx100Ftp.mpProj b/motoman_driver/MotoPlus/MpRosDx100Ftp.mpProj deleted file mode 100644 index 0d8cafb..0000000 --- a/motoman_driver/MotoPlus/MpRosDx100Ftp.mpProj +++ /dev/null @@ -1,34 +0,0 @@ - - - MpRosDx100Ftp - - Controller.c - Controller.h - CtrlGroup.c - CtrlGroup.h - MotionServer.c - MotionServer.h - mpMain.c - ParameterExtraction.h - ParameterExtraction_DX100_FTP.mpLib - SimpleMessage.c - SimpleMessage.h - StateServer.c - StateServer.h - - mpMain.c - False - Executable - - 192.168.32.3 - Net_Maint_Mng - 99999999 - - - mpbuilder\bin\ccpentium.exe -mcpu=pentiumiii -march=p3 -nostdlib -fno-builtin -fno-defer-pop -W -Werror-implicit-function-declaration -Wreturn-type -Wunused -Wuninitialized -Wunreachable-code -I"~ProjectDir~" -I"~IncludeDir~" -DCPU=PENTIUM4 -DDX100 -fvolatile -O -c "~FilePath~" -o "~OutputPath~" - mpbuilder\bin\ccpentium -nostdlib -r -Wl,-X -Wl~FileList~ -o "~OutputPath~" - C:\Program Files (x86)\Yaskawa\MotoPlusIDE_DX\mpbuilder\inc - - v2.0.2 - False - \ No newline at end of file diff --git a/motoman_driver/MotoPlus/MpRosFS100.fsProj b/motoman_driver/MotoPlus/MpRosFS100.fsProj deleted file mode 100644 index 9f117c5..0000000 --- a/motoman_driver/MotoPlus/MpRosFS100.fsProj +++ /dev/null @@ -1,37 +0,0 @@ - - - MpRosFS100 - - Controller.c - Controller.h - CtrlGroup.c - CtrlGroup.h - MotionServer.c - MotionServer.h - mpMain.c - ParameterExtraction.h - ParameterExtraction_FS100.fsLib - SimpleMessage.c - SimpleMessage.h - StateServer.c - StateServer.h - - Controller.h - False - Executable - - - - MOTOMANrobot - MOTOMANrobot - - - mpbuilder\gnu\4.1.2-vxworks-6.8\x86-win32\bin\ccppc.exe -g -te500v2 -fno-builtin -fsigned-char -Wall -Werror-implicit-function-declaration -DFS100 -DCPU=PPC32 -DTOOL_FAMILY=gnu -DTOOL=e500v2gnu -D_WRS_KERNEL -I"~ProjectDir~" -I"~IncludeDir~" -c "~FilePath~" -o "~OutputPath~" - - mpbuilder\gnu\4.1.2-vxworks-6.8\x86-win32\bin\ccppc.exe -nostdlib -r -WI,-X -WI~FileList~ -o "~OutputPath~" - - C:\Program Files (x86)\Yaskawa\MotoPlusIDE_FS\mpbuilder\inc - - v1.3.3 - False - \ No newline at end of file diff --git a/motoman_driver/MotoPlus/ParameterExtraction.dnLib b/motoman_driver/MotoPlus/ParameterExtraction.dnLib index b3fe98b..dda1222 100644 Binary files a/motoman_driver/MotoPlus/ParameterExtraction.dnLib and b/motoman_driver/MotoPlus/ParameterExtraction.dnLib differ diff --git a/motoman_driver/MotoPlus/ParameterExtraction.fsLib b/motoman_driver/MotoPlus/ParameterExtraction.fsLib index 32d3623..49fed96 100644 Binary files a/motoman_driver/MotoPlus/ParameterExtraction.fsLib and b/motoman_driver/MotoPlus/ParameterExtraction.fsLib differ diff --git a/motoman_driver/MotoPlus/ParameterExtraction.h b/motoman_driver/MotoPlus/ParameterExtraction.h index 1c21e73..ee8f035 100644 --- a/motoman_driver/MotoPlus/ParameterExtraction.h +++ b/motoman_driver/MotoPlus/ParameterExtraction.h @@ -196,6 +196,38 @@ extern STATUS GP_isBaxisSlave(int ctrlGrp, BOOL* bBaxisIsSlave); /******************************************************************************/ extern STATUS GP_getFeedbackSpeedMRegisterAddresses(int ctrlGrp, BOOL bActivateIfNotEnabled, BOOL bForceRebootAfterActivation, JOINT_FEEDBACK_SPEED_ADDRESSES* registerAddresses); +/******************************************************************************/ +/* << 22 >> */ +/* Function name : STATUS GP_isSdaRobot() */ +/* Functionality : Determines if the robot is a dual-arm SDA. */ +/* Parameter : BOOL* bIsSda - TRUE if robot is SDA [OUT] */ +/* Return value : Success = OK */ +/* : Failure = NG */ +/******************************************************************************/ +extern STATUS GP_isSdaRobot(BOOL* bIsSda); + +/******************************************************************************/ +/* << 23 >> */ +/* Function name : STATUS GP_isSharedBaseAxis() */ +/* Functionality : Determines if the robot is an SDA that has a base axis */ +/* which is shared over multiple control groups. */ +/* Parameter : BOOL* bIsSharedBaseAxis [OUT] */ +/* Return value : Success = OK */ +/* : Failure = NG */ +/******************************************************************************/ +extern STATUS GP_isSharedBaseAxis(BOOL* bIsSharedBaseAxis); + +/******************************************************************************/ +/* << 24 >> */ +/* Function name : STATUS GP_getDhParameters() */ +/* Functionality : Retrieves DH parameters for a given control group. */ +/* Parameter : int ctrlGrp - Robot control group (zero based index) [IN] */ +/* DH_PARAMETERS* dh - Value of the DH parameters [OUT] */ +/* Return value : Success = OK */ +/* : Failure = NG */ +/******************************************************************************/ +extern STATUS GP_getDhParameters(int ctrlGrp, DH_PARAMETERS* dh); + #ifdef __cplusplus } #endif diff --git a/motoman_driver/MotoPlus/ParameterExtraction.mpLib b/motoman_driver/MotoPlus/ParameterExtraction.mpLib index 61e0ee4..34eeda4 100644 Binary files a/motoman_driver/MotoPlus/ParameterExtraction.mpLib and b/motoman_driver/MotoPlus/ParameterExtraction.mpLib differ diff --git a/motoman_driver/MotoPlus/ParameterExtraction.yrcLib b/motoman_driver/MotoPlus/ParameterExtraction.yrcLib index 8c981c5..97f21a7 100644 Binary files a/motoman_driver/MotoPlus/ParameterExtraction.yrcLib and b/motoman_driver/MotoPlus/ParameterExtraction.yrcLib differ diff --git a/motoman_driver/MotoPlus/ParameterExtraction.yrcmlib b/motoman_driver/MotoPlus/ParameterExtraction.yrcmlib new file mode 100644 index 0000000..b0d7af5 Binary files /dev/null and b/motoman_driver/MotoPlus/ParameterExtraction.yrcmlib differ diff --git a/motoman_driver/MotoPlus/ParameterTypes.h b/motoman_driver/MotoPlus/ParameterTypes.h index 433adf3..b2a5bae 100644 --- a/motoman_driver/MotoPlus/ParameterTypes.h +++ b/motoman_driver/MotoPlus/ParameterTypes.h @@ -112,6 +112,19 @@ typedef struct ULONG cioAddressForAxis[MAX_PULSE_AXES][2]; } JOINT_FEEDBACK_SPEED_ADDRESSES; +typedef struct +{ + float theta; + float d; + float a; + float alpha; +} DH_LINK; + +typedef struct +{ + DH_LINK link[MAX_PULSE_AXES]; +} DH_PARAMETERS; + #ifdef __cplusplus } #endif diff --git a/motoman_driver/MotoPlus/RosSetupValidation.dnLib b/motoman_driver/MotoPlus/RosSetupValidation.dnLib deleted file mode 100644 index a9e79c4..0000000 Binary files a/motoman_driver/MotoPlus/RosSetupValidation.dnLib and /dev/null differ diff --git a/motoman_driver/MotoPlus/RosSetupValidation.fsLib b/motoman_driver/MotoPlus/RosSetupValidation.fsLib deleted file mode 100644 index 82e9066..0000000 Binary files a/motoman_driver/MotoPlus/RosSetupValidation.fsLib and /dev/null differ diff --git a/motoman_driver/MotoPlus/RosSetupValidation.h b/motoman_driver/MotoPlus/RosSetupValidation.h deleted file mode 100644 index de9408c..0000000 --- a/motoman_driver/MotoPlus/RosSetupValidation.h +++ /dev/null @@ -1,43 +0,0 @@ -//SetupValidation.h -// - -#define MOTOROS_SETUPERROR_ALARMCODE 8003 - -typedef enum -{ - MOTOROS_SETUP_OK = 0, //All is good - - MOTOROS_SETUP_RS0, //Set RS000 = 2 - - MOTOROS_SETUP_S2C541, //Set S2C541 = 0 - MOTOROS_SETUP_S2C542, //Set S2C542 = 0 - MOTOROS_SETUP_S2C1100, //Set S2C1100 = 1 - //MOTOROS_SETUP_S2C1101, //Set S2C1101 = 1 (recommended, but optional) - MOTOROS_SETUP_S2C1103, //Set S2C1103 = 2 - MOTOROS_SETUP_S2C1117, //Set S2C1117 = 1 - MOTOROS_SETUP_S2C1119, //Set S2C1119 = 0 (optionally can be =2 to allow telnet connections) - - MOTOROS_SETUP_NotCompatibleWithPFL, //Uninstall the PFL driver for the HC-10 - - //For all other error codes, please contact Yaskawa Motoman - //to have the MotoROS Runtime functionality enabled on - //your robot controller. - MOTOROS_SETUP_ContactYaskawaMotoman_1 = 100, - MOTOROS_SETUP_ContactYaskawaMotoman_2, - MOTOROS_SETUP_ContactYaskawaMotoman_3, - MOTOROS_SETUP_ContactYaskawaMotoman_4, - MOTOROS_SETUP_ContactYaskawaMotoman_5, - MOTOROS_SETUP_ContactYaskawaMotoman_6, - MOTOROS_SETUP_ContactYaskawaMotoman_7, - MOTOROS_SETUP_ContactYaskawaMotoman_8, - MOTOROS_SETUP_ContactYaskawaMotoman_9, - MOTOROS_SETUP_ContactYaskawaMotoman_10, - MOTOROS_SETUP_ContactYaskawaMotoman_11, - MOTOROS_SETUP_ContactYaskawaMotoman_12 -} MOTOROS_SETUP_CODES; - -// Verify most of the setup parameters of the robot controller. -// Please note that some parameters cannot be checked, such as -// the parameter(s) which enable this task to run. -extern MOTOROS_SETUP_CODES ValidateMotoRosSetupParameters(); - diff --git a/motoman_driver/MotoPlus/RosSetupValidation.mpLib b/motoman_driver/MotoPlus/RosSetupValidation.mpLib deleted file mode 100644 index 14b2ed1..0000000 Binary files a/motoman_driver/MotoPlus/RosSetupValidation.mpLib and /dev/null differ diff --git a/motoman_driver/MotoPlus/RosSetupValidation.yrcLib b/motoman_driver/MotoPlus/RosSetupValidation.yrcLib deleted file mode 100644 index 01e09e7..0000000 Binary files a/motoman_driver/MotoPlus/RosSetupValidation.yrcLib and /dev/null differ diff --git a/motoman_driver/MotoPlus/SimpleMessage.c b/motoman_driver/MotoPlus/SimpleMessage.c index 8ca11d8..ff018c8 100644 --- a/motoman_driver/MotoPlus/SimpleMessage.c +++ b/motoman_driver/MotoPlus/SimpleMessage.c @@ -132,25 +132,29 @@ int Ros_SimpleMsg_MotionReply(SimpleMsg* receiveMsg, int result, int subcode, Si replyMsg->header.commType = ROS_COMM_SERVICE_REPLY; replyMsg->header.replyType = ROS_REPLY_SUCCESS; + replyMsg->body.motionReply.groupNo = ctrlGrp; + // set reply body if(receiveMsg->header.msgType == ROS_MSG_MOTO_MOTION_CTRL) { - replyMsg->body.motionReply.groupNo = ctrlGrp; replyMsg->body.motionReply.sequence = receiveMsg->body.motionCtrl.sequence; replyMsg->body.motionReply.command = receiveMsg->body.motionCtrl.command; } else if(receiveMsg->header.msgType == ROS_MSG_JOINT_TRAJ_PT_FULL) { - replyMsg->body.motionReply.groupNo = ctrlGrp; replyMsg->body.motionReply.sequence = receiveMsg->body.jointTrajData.sequence; replyMsg->body.motionReply.command = ROS_MSG_JOINT_TRAJ_PT_FULL; } else if (receiveMsg->header.msgType == ROS_MSG_MOTO_JOINT_TRAJ_PT_FULL_EX) { - replyMsg->body.motionReply.groupNo = ctrlGrp; replyMsg->body.motionReply.sequence = receiveMsg->body.jointTrajDataEx.sequence; replyMsg->body.motionReply.command = ROS_MSG_MOTO_JOINT_TRAJ_PT_FULL_EX; } + if (receiveMsg->header.msgType == ROS_MSG_MOTO_SELECT_TOOL) + { + replyMsg->body.motionReply.sequence = receiveMsg->body.selectTool.sequence; + replyMsg->body.motionReply.command = receiveMsg->header.msgType; + } else { replyMsg->body.motionReply.groupNo = -1; diff --git a/motoman_driver/MotoPlus/SimpleMessage.h b/motoman_driver/MotoPlus/SimpleMessage.h index 79e2bfd..7920a8b 100644 --- a/motoman_driver/MotoPlus/SimpleMessage.h +++ b/motoman_driver/MotoPlus/SimpleMessage.h @@ -33,7 +33,8 @@ #define SIMPLE_MSG_H #define ROS_MAX_JOINT 10 -#define MOT_MAX_GR 4 +#define MOT_MAX_GR 4 + //---------------- // Prefix Section @@ -70,7 +71,10 @@ typedef enum ROS_MSG_MOTO_IOCTRL_REPLY = 2011, ROS_MSG_MOTO_JOINT_TRAJ_PT_FULL_EX = 2016, - ROS_MSG_MOTO_JOINT_FEEDBACK_EX = 2017 + ROS_MSG_MOTO_JOINT_FEEDBACK_EX = 2017, + ROS_MSG_MOTO_SELECT_TOOL = 2018, + + ROS_MSG_MOTO_GET_DH_PARAMETERS = 2020 } SmMsgType; @@ -133,7 +137,9 @@ typedef enum ROS_RESULT_INVALID_DATA_POSITION, ROS_RESULT_INVALID_DATA_SPEED, ROS_RESULT_INVALID_DATA_ACCEL, - ROS_RESULT_INVALID_DATA_INSUFFICIENT + ROS_RESULT_INVALID_DATA_INSUFFICIENT, + ROS_RESULT_INVALID_DATA_TIME, + ROS_RESULT_INVALID_DATA_TOOLNO } SmInvalidSubCode; @@ -149,7 +155,8 @@ typedef enum ROS_RESULT_NOT_READY_HOLD, ROS_RESULT_NOT_READY_NOT_STARTED, ROS_RESULT_NOT_READY_WAITING_ROS, - ROS_RESULT_NOT_READY_SKILLSEND + ROS_RESULT_NOT_READY_SKILLSEND, + ROS_RESULT_NOT_READY_PFL_ACTIVE } SmNotReadySubcode; @@ -257,6 +264,14 @@ struct _SmBodyJointFeedbackEx } __attribute__((__packed__)); typedef struct _SmBodyJointFeedbackEx SmBodyJointFeedbackEx; +struct _SmBodySelectTool +{ + int groupNo; // Robot/group ID; 0 = 1st robot + int tool; // Tool no for the selected group + int sequence; // Optional message tracking number that will be echoed back in the response. +} __attribute__((__packed__)); +typedef struct _SmBodySelectTool SmBodySelectTool; + //-------------- // IO Commands //-------------- @@ -321,6 +336,16 @@ struct _SmBodyMotoIoCtrlReply // ROS_MSG_MOTO_IOCTRL_REPLY = 2011 typedef struct _SmBodyMotoIoCtrlReply SmBodyMotoIoCtrlReply; //-------------- +// DH Parameters +//-------------- + +struct _SmBodyMotoGetDhParameters +{ + DH_PARAMETERS dhParameters[MOT_MAX_GR]; +} __attribute__((__packed__)); +typedef struct _SmBodyMotoGetDhParameters SmBodyMotoGetDhParameters; + +//-------------- // Body Union //-------------- @@ -333,6 +358,7 @@ typedef union SmBodyMotoMotionReply motionReply; SmBodyJointTrajPtFullEx jointTrajDataEx; SmBodyJointFeedbackEx jointFeedbackEx; + SmBodySelectTool selectTool; SmBodyMotoReadIOBit readIOBit; SmBodyMotoReadIOBitReply readIOBitReply; SmBodyMotoWriteIOBit writeIOBit; @@ -342,6 +368,7 @@ typedef union SmBodyMotoWriteIOGroup writeIOGroup; SmBodyMotoWriteIOGroupReply writeIOGroupReply; SmBodyMotoIoCtrlReply ioCtrlReply; + SmBodyMotoGetDhParameters dhParameters; } SmBody; //------------------- diff --git a/motoman_driver/MotoPlus/StateServer.c b/motoman_driver/MotoPlus/StateServer.c index fbe58fe..c5c0b5e 100644 --- a/motoman_driver/MotoPlus/StateServer.c +++ b/motoman_driver/MotoPlus/StateServer.c @@ -73,6 +73,8 @@ void Ros_StateServer_StartNewConnection(Controller* controller, int sd) //set feedback signal if(controller->tidStateSendState != INVALID_TASK) Ros_Controller_SetIOState(IO_FEEDBACK_STATESERVERCONNECTED, TRUE); + else + mpSetAlarm(8004, "MOTOROS FAILED TO CREATE TASK", 3); } break; diff --git a/motoman_driver/MotoPlus/YRC1000uCompilerArguments.mps b/motoman_driver/MotoPlus/YRC1000uCompilerArguments.mps new file mode 100644 index 0000000..153a573 --- /dev/null +++ b/motoman_driver/MotoPlus/YRC1000uCompilerArguments.mps @@ -0,0 +1 @@ +-march=atom -nostdlib -fno-builtin -fno-defer-pop -fno-implicit-fp -fno-zero-initialized-in-bss -Wall -Werror-implicit-function-declaration -g -MD -MP -DCPU=_VX_ATOM -DTOOL_FAMILY=gnu -DTOOL=gnu -DYRC1000u -D_WRS_KERNEL -I"~ProjectDir~" -I"~IncludeDir~" -O0 -c "~FilePath~" -o "~OutputPath~" diff --git a/motoman_driver/MotoPlus/YRC1000uLinkerArguments.mps b/motoman_driver/MotoPlus/YRC1000uLinkerArguments.mps new file mode 100644 index 0000000..6c84120 --- /dev/null +++ b/motoman_driver/MotoPlus/YRC1000uLinkerArguments.mps @@ -0,0 +1 @@ +-nostdlib -r -WI,-X -WI~FileList~ -o "~OutputPath~" diff --git a/motoman_driver/MotoPlus/_buildLog/MotoROSDX100.log b/motoman_driver/MotoPlus/_buildLog/MotoROSDX100.log deleted file mode 100644 index 7f7cb2d..0000000 --- a/motoman_driver/MotoPlus/_buildLog/MotoROSDX100.log +++ /dev/null @@ -1,15 +0,0 @@ - ------ Now building project: MotoRosDX100------ - Compiling Controller.c -Controller.c : warning : In function `Db_Print' -Controller.c(960): warning : unused parameter `msgFormat' - Compiling CtrlGroup.c -CtrlGroup.c : warning : In function `Ros_CtrlGroup_IsRobot' -CtrlGroup.c(613): warning : comparison of unsigned expression >= 0 is always true - Compiling IoServer.c - Compiling MotionServer.c - Compiling mpMain.c - Compiling SimpleMessage.c - Compiling StateServer.c - Building MotoRosDX100.out - Build Successful! Check 'DX100\MotoRosDX100' folder for MotoRosDX100.out - ------ diff --git a/motoman_driver/MotoPlus/_buildLog/MotoROSDX200.log b/motoman_driver/MotoPlus/_buildLog/MotoROSDX200.log deleted file mode 100644 index 9672302..0000000 --- a/motoman_driver/MotoPlus/_buildLog/MotoROSDX200.log +++ /dev/null @@ -1,11 +0,0 @@ - ------ Now building project: MotoRosDX200------ - Compiling Controller.c - Compiling CtrlGroup.c - Compiling IoServer.c - Compiling MotionServer.c - Compiling mpMain.c - Compiling SimpleMessage.c - Compiling StateServer.c - Building MotoRosDX200.out - Build Successful! Check 'DX200\MotoRosDX200' folder for MotoRosDX200.out - ------ diff --git a/motoman_driver/MotoPlus/_buildLog/MotoROSFS100.log b/motoman_driver/MotoPlus/_buildLog/MotoROSFS100.log deleted file mode 100644 index 2753d30..0000000 --- a/motoman_driver/MotoPlus/_buildLog/MotoROSFS100.log +++ /dev/null @@ -1,11 +0,0 @@ - ------ Now building project: MotoRosFS100------ - Compiling Controller.c - Compiling CtrlGroup.c - Compiling IoServer.c - Compiling MotionServer.c - Compiling mpMain.c - Compiling SimpleMessage.c - Compiling StateServer.c - Building MotoRosFS100.out - Build Successful! Check 'FS100\MotoRosFS100' folder for MotoRosFS100.out - ------ diff --git a/motoman_driver/MotoPlus/mpMain.c b/motoman_driver/MotoPlus/mpMain.c index b0b51af..859e084 100644 --- a/motoman_driver/MotoPlus/mpMain.c +++ b/motoman_driver/MotoPlus/mpMain.c @@ -63,7 +63,9 @@ void mpUsrRoot(int arg1, int arg2, int arg3, int arg4, int arg5, int arg6, int a //prototype will accept them. RosInitTaskID = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)RosInitTask, arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, arg10); - + if (RosInitTaskID == ERROR) + mpSetAlarm(8004, "MOTOROS FAILED TO CREATE TASK", 1); + //Ends the initialization task. mpExitUsrRoot; } @@ -83,6 +85,9 @@ void RosInitTask() ros_controller.tidConnectionSrv = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)Ros_Controller_ConnectionServer_Start, (int)&ros_controller, 0, 0, 0, 0, 0, 0, 0, 0, 0); + + if(ros_controller.tidConnectionSrv == ERROR) + mpSetAlarm(8004, "MOTOROS FAILED TO CREATE TASK", 2); #ifdef DX100 // DX100 need to execute a SKILLSEND command prior to the WAIT in order for the diff --git a/motoman_driver/MotoPlus/output/DX100/MotoRosDX100/MotoRosDX100.out b/motoman_driver/MotoPlus/output/DX100/MotoRosDX100/MotoRosDX100.out deleted file mode 100644 index d9f7792..0000000 Binary files a/motoman_driver/MotoPlus/output/DX100/MotoRosDX100/MotoRosDX100.out and /dev/null differ diff --git a/motoman_driver/MotoPlus/output/DX100/MotoRosDX1_/MotoRosDX1_190.out b/motoman_driver/MotoPlus/output/DX100/MotoRosDX1_/MotoRosDX1_190.out new file mode 100644 index 0000000..193166e Binary files /dev/null and b/motoman_driver/MotoPlus/output/DX100/MotoRosDX1_/MotoRosDX1_190.out differ diff --git a/motoman_driver/MotoPlus/output/DX200/MotoRosDX200/MotoRosDX200.out b/motoman_driver/MotoPlus/output/DX200/MotoRosDX200/MotoRosDX200.out deleted file mode 100644 index dd0d187..0000000 Binary files a/motoman_driver/MotoPlus/output/DX200/MotoRosDX200/MotoRosDX200.out and /dev/null differ diff --git a/motoman_driver/MotoPlus/output/DX200/MotoRosDX2_/MotoRosDX2_190.out b/motoman_driver/MotoPlus/output/DX200/MotoRosDX2_/MotoRosDX2_190.out new file mode 100644 index 0000000..810175e Binary files /dev/null and b/motoman_driver/MotoPlus/output/DX200/MotoRosDX2_/MotoRosDX2_190.out differ diff --git a/motoman_driver/MotoPlus/output/FS100/MotoRosFS100/MotoRosFS100.out b/motoman_driver/MotoPlus/output/FS100/MotoRosFS100/MotoRosFS100.out deleted file mode 100644 index 27c94af..0000000 Binary files a/motoman_driver/MotoPlus/output/FS100/MotoRosFS100/MotoRosFS100.out and /dev/null differ diff --git a/motoman_driver/MotoPlus/output/FS100/MotoRosFS_/MotoRosFS_190.out b/motoman_driver/MotoPlus/output/FS100/MotoRosFS_/MotoRosFS_190.out new file mode 100644 index 0000000..c79f1ba Binary files /dev/null and b/motoman_driver/MotoPlus/output/FS100/MotoRosFS_/MotoRosFS_190.out differ diff --git a/motoman_driver/MotoPlus/output/YRC1000/MotoRosYRC1000/MotoRosYRC1000.out b/motoman_driver/MotoPlus/output/YRC1000/MotoRosYRC1000/MotoRosYRC1000.out deleted file mode 100644 index 5507592..0000000 Binary files a/motoman_driver/MotoPlus/output/YRC1000/MotoRosYRC1000/MotoRosYRC1000.out and /dev/null differ diff --git a/motoman_driver/MotoPlus/output/YRC1000/MotoRosYRC1_/MotoRosYRC1_190.out b/motoman_driver/MotoPlus/output/YRC1000/MotoRosYRC1_/MotoRosYRC1_190.out new file mode 100644 index 0000000..0e56301 Binary files /dev/null and b/motoman_driver/MotoPlus/output/YRC1000/MotoRosYRC1_/MotoRosYRC1_190.out differ diff --git a/motoman_driver/MotoPlus/output/YRC1000u/MotoRosYRC1u_/MotoRosYRC1u_190.out b/motoman_driver/MotoPlus/output/YRC1000u/MotoRosYRC1u_/MotoRosYRC1u_190.out new file mode 100644 index 0000000..afdcf41 Binary files /dev/null and b/motoman_driver/MotoPlus/output/YRC1000u/MotoRosYRC1u_/MotoRosYRC1u_190.out differ ```

[gavanderhoorn]

In v1.7.2, velocity feedback support was added to MotoROS for FS100 (#205).

In v1.9.0 some changes were made to the "parameter extraction functions when using four control groups on an FS100" (#284).

[gavanderhoorn]

@impet14: was v1.7.0 the first version of MotoROS that started working? Have you tried any of the versions in between v1.7.0 and v1.9.0?

[impet14]

@gavanderhoorn , Thank you for your response. I have not tried the version between v1.7.0 and v1.9.0. When I use v1.7.0 and telnet to fs100, there is no error. But when I install v1.9.0 to fs100 there would be the error "Failed to update controller status. Check robot parameters." from telnet without connecting stream launch file.

[EricMarcil]

Thanks for the information. We are looking into it but I don't have access to an FS100 so I had to ask for assistance from a different office which is causing delay. We were also focusing on the controller setup but now it seems that is would be related to some change in the code. I'll try to follow-up on it and look at the code between 1.70 and 1.9.0 for the differences.

[EricMarcil]

I've identified the source of the problem with this morning information that version 1.7.0 was working, We added new I/O check for the PFL function (used by collaborative robots) but those I/O signals don’t exist for older controller models. I’ve put conditional compile around those modifications and recompiled it. @acbuynak tested it and confirm that the issue was solved. I'll create a PR for version 1.9.1 that will fix this problem.

[acbuynak]

Confirming that the proposed version 1.9.1 fixed all errors identified in this thread. I've tried a variety of commands, settings, and etc. Everything seems to be working as intended.

I was able to confirm message receipt by the FS100 using output from telnet.

[impet14]

Thanks. I look forward to test v1.9.1. Now I am using v1.8.2.

[gavanderhoorn]

Just merged #320 which should fix this.

[gavanderhoorn]

@alexander-jh: v1.9.1 has fixed this for you as well?

[alexander-jh]

@gavanderhoorn yes, @acbuynak and I work in the same lab.