Merge pull request #229 from yai-rosejo/issue_203_auto_switch_cycle

Issue 203 auto switch cycle mode

doc/ros_api.md

@@ -111,7 +111,8 @@ Note: errors and alarms which require physical operator intervention (e-stops, e
 
 Type: [motoros2_interfaces/srv/StartTrajMode](https://github.com/yaskawa-global/motoros2_interfaces/blob/d6805d32714df4430f7db3d8ddc736c340ddeba8/srv/StartTrajMode.srv)
 
-Attempt to enable servo drives and activate trajectory mode, allowing the action server (`follow_joint_trajectory`, see below) to execute incoming `FollowJointTrajectory` action goals.
+Attempts to enable servo drives, activate trajectory mode, and set the job-cycle mode to allow execution of INIT_ROS.
+This allows the action server (`follow_joint_trajectory`, see below) to execute incoming `FollowJointTrajectory` action goals.
 
 Note: this service may fail if controller state prevents it from transitioning to trajectory mode.
 Inspect the `result_code` to determine the cause.
@@ -123,7 +124,8 @@ The `reset_error` service can be used to attempt to reset errors and alarms.
 
 Type: [motoros2_interfaces/srv/StartPointQueueMode](https://github.com/yaskawa-global/motoros2_interfaces/blob/d6805d32714df4430f7db3d8ddc736c340ddeba8/srv/StartPointQueueMode.srv)
 
-Attempt to enable servo drives and activate the point-queue motion mode, allowing the `queue_traj_point` service to execute incoming `QueueTrajPoint` requests.
+Attempts to enable servo drives, activate the point-queue motion mode, and set the job-cycle mode to allow execution of INIT_ROS.
+This allows the `queue_traj_point` service (see below) to execute incoming `QueueTrajPoint` requests.
 
 Note: this service may fail if controller state prevents it from transitioning to trajectory mode.
 Inspect the `result_code` to determine the cause.

doc/troubleshooting.md

@@ -1117,3 +1117,26 @@ ALARM 8015
 *Solution:*
 Open a new ticket on the MotoROS2 [Issue tracker](https://github.com/yaskawa-global/motoros2/issues).
 Please include a copy of the `RBCALIB.DAT` from your robot controller along with the output from the [Debug log client](#debug-log-client).
+
+### Alarm: 8016[0]
+
+*Example:*
+
+```text
+ALARM 8016
+ Set job-cycle to AUTO
+[0]
+```
+
+*Solution:*
+The job cycle is currently set to `STEP` and MotoROS2 was unable to automatically change it to `AUTO`.
+This will prevent the `INIT_ROS` from operating continuously and will prevent the software from accepting any incoming trajectories.
+
+ 1. upgrade to *MANAGEMENT* security level by touching `[System Info]`→`[Security]` (default password is all `9`'s)
+ 1. touch `[Job]`→`[Cycle]`
+ 1. change `WORK SELECT` to `AUTO`
+ 1. touch `[Setup]`→`[Operate Cond.]`
+ 1. change `CYCLE SWITCH IN REMOTE MODE` to `AUTO`
+
+If the problem persists, verify that the `CIOPRG.LST` ladder program is not writing to `#40050 - #40052`.
+Please contact Yaskawa Technical Support for assistance if you are not familiar with the Concurrent I/O Ladder Program.

lib/MotoPlusExterns.h

@@ -32,4 +32,10 @@ extern MP_GRP_ID_TYPE mpCtrlGrpNo2GrpId(int grp_no);
 #define E_EXRCS_UNDER_ENERGY_SAVING         (-20)
 #endif
 
+//from the M+ API Function Specifications on mpSetCycle(..) (HW1483602)
+#define MP_CYCLE_MODE_STEP 1
+#define MP_CYCLE_MODE_1CYCLE 2
+#define MP_CYCLE_MODE_AUTO 3
+
+
 #endif // MOTOROS2_MOTOPLUS_EXTERNS_H

src/ErrorHandling.h

@@ -82,6 +82,7 @@ typedef enum
     ALARM_CONFIGURATION_FAIL,
     ALARM_INFORM_JOB_FAIL,
     ALARM_DAT_FILE_PARSE_FAIL,
+    ALARM_OPERATION_FAIL,
 } ALARM_MAIN_CODE;
 
 
@@ -217,6 +218,11 @@ typedef enum
     SUBCODE_DAT_FAIL_PARSE_SRANG
 } ALARM_DAT_FILE_PARSE_FAIL_SUBCODE; //8015
 
+typedef enum
+{
+    SUBCODE_OPERATION_SET_CYCLE,
+} ALARM_OPERATION_FAIL_SUBCODE; //8016
+
 extern void motoRosAssert(BOOL mustBeTrue, ALARM_ASSERTION_FAIL_SUBCODE subCodeIfFalse);
 extern void motoRosAssert_withMsg(BOOL mustBeTrue, ALARM_ASSERTION_FAIL_SUBCODE subCodeIfFalse, char* msgFmtIfFalse, ...);
 

src/MotionControl.c

@@ -1355,6 +1355,7 @@ static STATUS Ros_Controller_DisableEcoMode()
 //
 // NOTE: only attempts to start job if necessary, does not reset errors, alarms.
 //       Does attempt to enable servo power (if not on)
+//       Does attempt to set the cycle mode to continuous (if not set)
 //-----------------------------------------------------------------------
 BOOL Ros_MotionControl_StartMotionMode(MOTION_MODE mode)
 {
@@ -1392,13 +1393,6 @@ BOOL Ros_MotionControl_StartMotionMode(MOTION_MODE mode)
     }
 #endif
 
-    // Check if in continous cycle mode
-    if (!Ros_Controller_IsContinuousCycle())
-    {
-        Ros_Debug_BroadcastMsg("Continuous cycle mode not set, can't enable trajectory mode");
-        return FALSE;
-    }
-
     if (Ros_Controller_IsAnyFaultActive())
     {
         Ros_Debug_BroadcastMsg("Controller is in a fault state. Please call /reset_error");
@@ -1436,6 +1430,35 @@ BOOL Ros_MotionControl_StartMotionMode(MOTION_MODE mode)
         }
     }
 
+    // Check if in continous cycle mode
+    if (!Ros_Controller_IsContinuousCycle())
+    {
+        // set the cycle mode to auto if not currently
+        MP_CYCLE_SEND_DATA sCycleData;
+        bzero(&sCycleData, sizeof(sCycleData));
+        bzero(&rData, sizeof(rData));
+        sCycleData.sCycle = MP_CYCLE_MODE_AUTO;
+        ret = mpSetCycle(&sCycleData, &rData);
+        if( (ret != 0) || (rData.err_no != 0) ) 
+        {
+            Ros_Debug_BroadcastMsg(
+                "Can't set cycle mode to continuous because: '%s' (0x%04X)",
+                Ros_ErrorHandling_ErrNo_ToString(rData.err_no), rData.err_no);
+            mpSetAlarm(ALARM_OPERATION_FAIL, "Set job-cycle to AUTO", SUBCODE_OPERATION_SET_CYCLE);
+            return FALSE;
+        }
+
+        Ros_Sleep(g_Ros_Controller.interpolPeriod); //give CIO time to potentially overwrite the cycle (Ladder scan time is smaller than the interpolPeriod)
+        Ros_Controller_IoStatusUpdate(); //verify the cycle got set and wasn't forced back due to CIO logic
+
+        if (!Ros_Controller_IsContinuousCycle())
+        {
+            Ros_Debug_BroadcastMsg("Can't set cycle mode. Check CIOPRG.LST for OUT #40050 - #40052");
+            mpSetAlarm(ALARM_OPERATION_FAIL, "Set job-cycle to AUTO", SUBCODE_OPERATION_SET_CYCLE);
+            return false;
+        }
+    }
+
 #ifndef DUMMY_SERVO_MODE
     // Servo On
     if(Ros_Controller_IsServoOn() == FALSE)