PLCopen Motion Control

The PLCopen* motion standard provides a way to have standard application libraries that are reusable for multiple hardware platforms. It is widely accepted in the industrial control market for the CNC, robotics, and motion control applications. For more detailed information of the PLCopen motion specifications, refer to Motion Control.

Install PLCopen Motion Control

You can install this component from the ECI repository. Setup the ECI repository, then perform either of the following commands to install this component:

Install from meta-package
$ sudo apt install eci-softplc-plcopen
Install from individual Deb package
# For non-Xenomai kernels
$ sudo apt install plcopen-motion plcopen-servo plcopen-databus \
  plcopen-benchmark plcopen-ruckig plcopen-openplc-runtime plcopen-openplc-editor

# For Xenomai kernels
$ sudo apt install plcopen-motion-xenomai plcopen-servo-xenomai plcopen-databus-xenomai \
  plcopen-benchmark-xenomai plcopen-ruckig plcopen-openplc-runtime plcopen-openplc-editor

Install PLCopen Motion Control Development Headers

This component also provides header files for development use. Setup the ECI repository, then perform the following command to install this component:

Install from individual Deb package
# For non-Xenomai kernels
$ sudo apt install plcopen-motion-dev plcopen-servo-dev \
  plcopen-ruckig-dev plcopen-databus-dev plcopen-benchmark-dev

# For Xenomai kernels
$ sudo apt install plcopen-motion-xenomai-dev plcopen-servo-xenomai-dev \
  plcopen-ruckig-dev plcopen-databus-xenomai-dev plcopen-benchmark-xenomai-dev

PLCopen Motion Function Blocks

In Intel® Edge Controls for Industrial (Intel® ECI or ECI), the PLCopen motion control library RTmotion is used with the IgH EtherCAT Master Stack and SoftPLC to create general motion control applications. RTmotion is an open source C++ library created by Intel®. RTmotion implements parts 1 and 2 of PLCopen motion control standard. With the extensions of machine vision and ROS2 functions, RTmotion function blocks have been successfully deployed to control discrete automation equipment, autonomous mobile robots (AMR) and robotic arms. The following figure shows the connection between RTmotion and other components in ECI.

../_images/soft_motion_arch.png

The following sections provide overview on how to use the RTmotion function blocks to develop motion control applications for industrial automation and robotics.

Note

For the detailed information about RTmotion, Please visit RTmotion documentation.

Applications for Industrial Automation

Labeling is commonly seen in the automation of the pharmaceuticals, beverage, and chemicals packaging. As shown in the following figures (left 1 and right 2), the task is to place a label at a particular position on a product. The application has two drives, one to feed the product via a conveyor belt and another to feed the labels and to place the labels on the products. The labeling process is triggered by a position detection sensor. From the detection of the product to the start of the label movement there is a delay depending on the velocity of the conveyor, the position of the sensor, and the position of the label on the product.

fig1 fig2

RTmotion motion control function blocks can be used with the SoftPLC systems, for example, CODESYS, MULTIPROG, and LogicLab, to solve this kind of problems. SoftPLC generally supports five programming languages for the application development according to the IEC 61131-3 standard: Instruction List (IL), Structured Text (ST), Function Block Diagram (FBD), Ladder Diagram (LD), and Sequential Function Chart (SFC). The following FBD 2 shows a way to solve this task. Both axes move with the same velocity setpoint. The delay for On Delay Timer (TON) is calculated from the sensor distance and the velocity. After a labeling step, the LabelDrive stops again and waits for the next trigger, while the conveyor continuously moves.

../_images/plcopen_part_3_demo_FBD.PNG

The labeling application can also be implemented as an ST program:

Click to view the ST program

(* PLC configuration *)
CONFIGURATION DefaultCfg
  VAR_GLOBAL
    Product_Detection   AT %MX3.0.0: BOOL;
    Start               AT %MX3.1.0: BOOL;
    Label_Length        AT %ML3.200: REAL;
    Sensor_Distance     AT %ML3.204: REAL;
    Velocity            AT %ML3.208: REAL;
  END_VAR

  // Schedule the main program to be executed every 1 ms
  TASK Tick(INTERVAL := T#1ms);

  PROGRAM Main WITH Tick : Labeling_Example_Code;
END_CONFIGURATION

(* Labeling machine program *)
PROGRAM Labeling_Example_Code
  VAR_EXTERNAL
    Product_Detection  : BOOL;
    Start              : BOOL;
    Label_Length:      : REAL;
    Sensor_Distance:   : REAL;
    Velocity           : REAL;
  END_VAR

  VAR
    MC_Power_1:         MC_Power;
    MC_Power_2:         MC_Power;
    Label_Drive:        AXIS_REF;
    Conveyor:           AXIS_REF;
    MC_MoveVelocity_1:  MC_MoveVelocity;
    MC_MoveRelative_1:  MC_MoveRelative;
    Delay_Timer:        TON;
  END_VAR

  MC_Power_1(Axis := Label_Drive,
            Enable := True,
            EnablePositive := True,
            EnableNegative := True);

  MC_Power_2(Axis := Conveyor,
            Enable := True,
            EnablePositive := True,
            EnableNegative := True);

  Delay_Timer(IN := Product_Detection,
              PT := REAL_TO_TIME(IN := Sensor_Distance / Velocity));

  IF MC_Power_1.Status AND MC_Power_1.Valid THEN
    MC_MoveRelative_1(Axis := Label_Drive,
                      Execute := Delay_Timer.Q,
                      Position := Label_Length,
                      Velocity := Velocity);
  END_IF;

  IF MC_Power_2.Status AND MC_Power_2.Valid THEN
    MC_MoveVelocity_1(Axis := Conveyor,
                      Execute := Start,
                      Velocity := Velocity);
  END_IF;
END_PROGRAM

A sample is provided in ECI, which converts the ST program into a C++ program and drives two Inovance EtherCAT servo motors. Run by the following command:

$ /opt/plcopen/duo_rtmotion_demo -n inovance_duo_1ms.xml -i 1000

where, -n specifies the EtherCAT ENI file, which is generated from a vendor-specific EtherCAT ESI file. For more information, refer to EtherCAT Enablekit. -i specifies the real-time cycle time in microseconds.

To run a real labeling application, input the FBD or ST program to the SoftPLC system, integrate the RTmotion function blocks with the SoftPLC system, and connect the function blocks to the servo motors through the IgH* EtherCAT master stack. Then, use the SoftPLC system to compile and run the program to control the labeling machine.

SoftPLC systems usually provide an interface to integrate external function blocks written in C or C++. The integration includes three steps:

  1. Develop a new derived servo class from the RTmotion/Servo class. Override all the virtual functions of the RTmotion/Servo class by using the SoftPLC provided EtherCAT master stack API to control a servo motor.

  2. Use SoftPLC SDK to create custom function blocks that match the input/output of RTmotion::Function Blocks.

  3. Register the initialization of RTmotion::Axes, RTmotion::Function Blocks and RTmotion::Derived Servos to the PLC initialization function. Register the runCycle functions of RTmotion::Axes and RTmotion::Function Blocks to the PLC runtime functions.

When running the FBD or ST program, the function blocks will be called to send the trajectory planning commands to control the servo motors.

Applications for Robotics

Robot Operating System (ROS/2) 3 is the worldwide de facto standard software middleware for developing intelligent robot applications. With specially designed interface to ROS/2, RTmotion motion control function blocks can be combined with the ROS/2 Navigation and MoveIt to develop applications for the (AMR) and industrial robots, as shown below. This opens up the potential that real-time low-level control and intelligent high-level control to run seamlessly on a single Intel platform.

../_images/robotics_app_1.png

In ECI, the concept has been proven in the joint-lab project between SYMG and Intel through controlling an omni-directional AMR with four servo motors by integrating ROS2 navigation, SoftPLC (commercial and open sourced), PLCopen motion function blocks, and IgH EtherCAT master stack, and running the whole software stack on ECI optimized real-time Linux kernel.

../_images/symg_amr_arch.png

As shown in the above figure, ROS2 navigation plans the global path routine and local trajectory for the AMR to move. When AMR moves, the velocity command cmd_vel, calculated by ROS2 Navigation, is transmitted to the SoftPLC system through the shared memory. This guarantees the efficient communication and good separation between real-time and non real-time processes. The velocity command cmd_vel includes three components indicating the velocities translating forward (VelX), translating sideways (VelY), and rotating round the center of mass (VelRZ) on the 2-D plane. The AMR velocity command is parsed and split into the velocities of four Mecanum 4 wheels (UL, UR, LL, LR). The following figure shows the details of this split process. The velocity command of each wheel is input into a MC_MoveVelocty function block to execute. The MC_MoveVelocity function block makes path interpolation between the velocity inputs and sends the command to the servo motor through the IgH EtherCAT stack. At the same time, the SoftPLC calls the function blocks MC_ReadActualPosition and MC_ReadActualVelocity to read the wheel velocity, and synthesize the information into the odometry of AMR. The odometry information is then transmitted back to ROS2 navigation through the shared memory for the SLAM calculation.

../_images/robotics_app_2.png

The following is the ST program for this application, which is configured to run in 1ms cycle:

Click to view the ST program

(* Split the AMR velocity into the wheel velocities *)
FUNCTION_BLOCK AGVM_VelCartesian2Wheels
  VAR_INPUT
    VelX :    LREAL;
    VelY :    LREAL;
    VelRZ :   LREAL;
  END_VAR

  VAR_OUTPUT
    UL :      LREAL;
    UR :      LREAL;
    LL :      LREAL;
    LR :      LREAL;
  END_VAR

  VAR_EXTERNAL
    WheelHDistance :  LREAL;(**)
    WheelVDistance :  LREAL;(**)
  END_VAR

  VAR
    VelLZ :   LREAL;
  END_VAR

  VelLZ := VelRZ * ((WheelHDistance + WheelVDistance) / LREAL#2.0);

  UL := VelX - VelY - VelLZ;
  UR := VelX + VelY + VelLZ;
  LL := VelX + VelY - VelLZ;
  LR := VelX - VelY + VelLZ;
END_FUNCTION_BLOCK

(* Synthesize the wheel velocities into the AMR velocity *)
FUNCTION_BLOCK AGVM_VelWheels2Cartesian
  VAR_INPUT
    UL :      LREAL;
    UR :      LREAL;
    LL :      LREAL;
    LR :      LREAL;
  END_VAR

  VAR_OUTPUT
    VelX :    LREAL;
    VelY :    LREAL;
    VelRZ :   LREAL;
  END_VAR

  VAR_EXTERNAL
    WheelHDistance :  LREAL;(**)
    WheelVDistance :  LREAL;(**)
  END_VAR

  VelX := (UL + UR + LL + LR) / LREAL#4.0;

  VelY := (-UL + UR + LL - LR) / LREAL#4.0;
  VelRZ := (-UL + UR - LL + LR) / LREAL#4.0 / ((WheelHDistance + WheelVDistance) / LREAL#2.0);
END_FUNCTION_BLOCK

(* Global variables *)
VAR_GLOBAL
  PLC_SYS_TICK_CNT    AT %MD1.0 :     UDINT;
  PLC_TASK_DEFINED    AT %MW1.4 :     INT;
  PLCMODE_ON  AT %MX1.2016.0 :        BOOL;(*TRUE : current PLC mode is ON*)
  PLCMODE_LOADING     AT %MX1.2017.0 :        BOOL;(*TRUE : current PLC mode is LOADING*)
  PLCMODE_STOP        AT %MX1.6.0 :   BOOL;(*TRUE : current PLC mode is STOP*)
  PLCMODE_RUN AT %MX1.7.0 :   BOOL;(*TRUE : current PLC mode is RUN*)
  PLCMODE_HALT        AT %MX1.8.0 :   BOOL;(*TRUE : current PLC mode is HALT*)
  PLC_TICKS_PER_SEC   AT %MW1.2000 :  UINT;
  PLC_MAX_ERRORS      AT %MD1.2004 :  UDINT;
  PLC_ERRORS  AT %MD1.2008 :  UDINT;
  PLC_TASK_AVAILABLE  AT %MW1.2012 :  INT;
  PLC_SYSTASK_AVAILABLE       AT %MW1.2016 :  INT;
  PLCDEBUG_FORCE      AT %MX1.2018.0 :        BOOL;(*TRUE : current PLC mode is POWER on*)
  PLCDEBUG_BPSET      AT %MX1.2019.0 :        BOOL;(*TRUE : one or more*)
  PLCDEBUG_POWERFLOW  AT %MX1.2020.0 :        BOOL;(*TRUE : current PLC mode is POWER on*)
  WheelVDistance :    LREAL := 0.483;(*Distance between the front and back wheels*)
  WheelHDistance :    LREAL := 0.371;(*Distance between the two side wheels*)
END_VAR

(* Main program *)
PROGRAM main
  VAR
    Acc :     LREAL := 2.0;
    MC_Power_1 :      MC_Power;
    MC_Power_2 :      MC_Power;
    MC_Power_3 :      MC_Power;
    MC_Power_4 :      MC_Power;
    PowStat : BOOL;
    AxisLL :  DINT := 3;
    AxisLR :  DINT := 4;
    AxisUL :  DINT := 1;
    AxisUR :  DINT := 2;
    RatioLimit :      LREAL := 1.0;
    VAxisULVel :      LREAL;
    VAxisURVel :      LREAL;
    VAxisLLVel :      LREAL;
    VAxisLRVel :      LREAL;
    VAxisMaxVel :     LREAL;
    AxisVelLimit :    LREAL := 0.75;
    MC_EmergencyStop_1 :      MC_EmergencyStop;
    MC_Reset_1 :      MC_Reset;
    TransH :  LREAL;
    TransV :  LREAL;
    Twist :   LREAL;
    MC_Reset_2 :      MC_Reset;
    MC_Reset_3 :      MC_Reset;
    MC_Reset_4 :      MC_Reset;
    ENABLE_FLAG :     BOOL;
    MC_Halt_2 :       MC_Halt;
    MC_Halt_3 :       MC_Halt;
    MC_Halt_4 :       MC_Halt;
    MC_MoveVelocity_2 :       MC_MoveVelocity;
    MC_MoveVelocity_3 :       MC_MoveVelocity;
    MC_MoveVelocity_4 :       MC_MoveVelocity;
    MC_Halt_1 :       MC_Halt;
    MC_MoveVelocity_1 :       MC_MoveVelocity;
    SHM_PosX  AT %ML3.200 :   LREAL := 0.0;
    SHM_PosY  AT %ML3.208 :   LREAL := 0.0;
    SHM_PosRZ AT %ML3.216 :   LREAL := 0.0;
    SHM_VelX  AT %ML3.224 :   LREAL := 0.0;
    SHM_VelY  AT %ML3.232 :   LREAL := 0.0;
    SHM_VelRZ AT %ML3.240 :   LREAL := 0.0;
    SHM_Error AT %MX3.248.0 : BOOL;
    MC_ReadAxisError_1 :      MC_ReadAxisError;
    SHM_Enable        AT %MX3.100.0 : BOOL;
    SHM_EmergStop     AT %MX3.101.0 : BOOL;
    SHM_TransH        AT %ML3.102 :   LREAL := 0.0;
    SHM_Twist AT %ML3.118 :   LREAL := 0.0;
    SHM_TransV        AT %ML3.110 :   LREAL := 0.0;
    MC_SetPosition_1 :        MC_SetPosition;
    MC_ReadActualPosition_1 : MC_ReadActualPosition;
    AGVM_VelCartesian2Wheels_1 :      AGVM_VelCartesian2Wheels;
    MC_ReadActualVelocity_1 : MC_ReadActualVelocity;
    MC_ReadActualVelocity_2 : MC_ReadActualVelocity;
    MC_ReadActualVelocity_3 : MC_ReadActualVelocity;
    MC_ReadActualVelocity_4 : MC_ReadActualVelocity;
    AGVM_VelWheels2Cartesian_1 :      AGVM_VelWheels2Cartesian;
    AxisULPos :       LREAL;
    MC_ReadActualPosition_2 : MC_ReadActualPosition;
    MC_ReadActualPosition_3 : MC_ReadActualPosition;
    MC_ReadActualPosition_4 : MC_ReadActualPosition;
    AxisURPos :       LREAL;
    AxisLLPos :       LREAL;
    AxisLRPos :       LREAL;
    AGVM_VelWheels2Cartesian_2 :      AGVM_VelWheels2Cartesian;
    AxisULErr :       WORD;
    AxisURErr :       WORD;
    AxisLLErr :       WORD;
    AxisLRErr :       WORD;
    INIT :    BOOL := TRUE;
    MC_MoveAbsolute_1 :       MC_MoveAbsolute;
  END_VAR

  TransV := SHM_TransV;
  TransH := SHM_TransH;
  Twist := SHM_Twist;

  IF SHM_EmergStop THEN
    SHM_EmergStop := FALSE;
    SHM_Enable := FALSE;
    ENABLE_FLAG:=FALSE;
    MC_EmergencyStop_1(Execute:=FALSE);
    MC_EmergencyStop_1(Axis:=AxisUL,Execute:=TRUE);
    MC_EmergencyStop_1(Execute:=FALSE);
    MC_EmergencyStop_1(Axis:=AxisUR,Execute:=TRUE);
    MC_EmergencyStop_1(Execute:=FALSE);
    MC_EmergencyStop_1(Axis:=AxisLL,Execute:=TRUE);
    MC_EmergencyStop_1(Execute:=FALSE);
    MC_EmergencyStop_1(Axis:=AxisLR,Execute:=TRUE);
  END_IF;

  IF SHM_Enable THEN
    SHM_Enable := FALSE;
    IF NOT PowStat THEN
      MC_Reset_1(Execute:=FALSE);
      MC_Reset_2(Execute:=FALSE);
      MC_Reset_3(Execute:=FALSE);
      MC_Reset_4(Execute:=FALSE);
      ENABLE_FLAG:=TRUE;
    END_IF;
  END_IF;

  IF ENABLE_FLAG THEN
    MC_Reset_1(Axis:=AxisUL,Execute:=TRUE);
    MC_Reset_2(Axis:=AxisUR,Execute:=TRUE);
    MC_Reset_3(Axis:=AxisLL,Execute:=TRUE);
    MC_Reset_4(Axis:=AxisLR,Execute:=TRUE);

    IF MC_Reset_1.Done AND MC_Reset_2.Done AND MC_Reset_3.Done AND MC_Reset_4.Done THEN
      ENABLE_FLAG := FALSE;
    END_IF;
  END_IF;

  MC_Power_1(Axis:=AxisUL,Enable:=TRUE,EnablePositive:=TRUE,EnableNegative:=TRUE);
  MC_Power_2(Axis:=AxisUR,Enable:=TRUE,EnablePositive:=TRUE,EnableNegative:=TRUE);
  MC_Power_3(Axis:=AxisLL,Enable:=TRUE,EnablePositive:=TRUE,EnableNegative:=TRUE);
  MC_Power_4(Axis:=AxisLR,Enable:=TRUE,EnablePositive:=TRUE,EnableNegative:=TRUE);

  PowStat := MC_Power_1.Status AND MC_Power_1.Valid AND
        MC_Power_2.Status AND MC_Power_2.Valid AND
        MC_Power_3.Status AND MC_Power_3.Valid AND
        MC_Power_4.Status AND MC_Power_4.Valid;

  IF PowStat THEN
    IF INIT THEN
      MC_SetPosition_1(Execute:=FALSE);
      MC_SetPosition_1(Axis:=AxisUL,Execute:=TRUE,Position:=LREAL#0.0,Relative:=FALSE,Source:=DINT#1);
      MC_SetPosition_1(Execute:=FALSE);
      MC_SetPosition_1(Axis:=AxisUR,Execute:=TRUE,Position:=LREAL#0.0,Relative:=FALSE,Source:=DINT#1);
      MC_SetPosition_1(Execute:=FALSE);
      MC_SetPosition_1(Axis:=AxisLL,Execute:=TRUE,Position:=LREAL#0.0,Relative:=FALSE,Source:=DINT#1);
      MC_SetPosition_1(Execute:=FALSE);
      MC_SetPosition_1(Axis:=AxisLR,Execute:=TRUE,Position:=LREAL#0.0,Relative:=FALSE,Source:=DINT#1);
      SHM_PosX:=LREAL#0.0;
      SHM_PosY:=LREAL#0.0;
      SHM_PosRZ:=LREAL#0.0;
      AxisULPos:=LREAL#0.0;
      AxisURPos:=LREAL#0.0;
      AxisLLPos:=LREAL#0.0;
      AxisLRPos:=LREAL#0.0;
      INIT:=FALSE;
    END_IF;

    AGVM_VelCartesian2Wheels_1(VelX:=TransV,VelY:=TransH,VelRZ:=Twist);
    VAxisULVel:=AGVM_VelCartesian2Wheels_1.UL;
    VAxisURVel:=AGVM_VelCartesian2Wheels_1.UR;
    VAxisLLVel:=AGVM_VelCartesian2Wheels_1.LL;
    VAxisLRVel:=AGVM_VelCartesian2Wheels_1.LR;

    RatioLimit := LREAL#1.0;
    VAxisMaxVel := ABS(VAxisULVel);

    IF VAxisMaxVel < ABS(VAxisURVel) THEN
      VAxisMaxVel := ABS(VAxisURVel);
    END_IF;

    IF VAxisMaxVel < ABS(VAxisLLVel) THEN
      VAxisMaxVel := ABS(VAxisLLVel);
    END_IF;

    IF VAxisMaxVel < ABS(VAxisLRVel) THEN
      VAxisMaxVel := ABS(VAxisLRVel);
    END_IF;

    IF VAxisMaxVel = LREAL#0.0 THEN
      RatioLimit := LREAL#0.0;
    ELSIF VAxisMaxVel > AxisVelLimit THEN
      RatioLimit := AxisVelLimit / VAxisMaxVel;
    END_IF;

    IF VAxisULVel = LREAL#0.0 THEN
      MC_Halt_1(Execute:=FALSE);
      MC_Halt_1(Axis:=AxisUL,Execute:=TRUE,Deceleration:=Acc);
    ELSE
      MC_MoveVelocity_1(Execute:=FALSE);
      MC_MoveVelocity_1(Axis:=AxisUL,Execute:=TRUE,Velocity:=VAxisULVel * RatioLimit,Acceleration:=Acc,Deceleration:=Acc);
    END_IF;

    IF VAxisURVel = LREAL#0.0 THEN
      MC_Halt_2(Execute:=FALSE);
      MC_Halt_2(Axis:=AxisUR,Execute:=TRUE,Deceleration:=Acc);
    ELSE
      MC_MoveVelocity_2(Execute:=FALSE);
      MC_MoveVelocity_2(Axis:=AxisUR,Execute:=TRUE,Velocity:=VAxisURVel * RatioLimit,Acceleration:=Acc,Deceleration:=Acc);
    END_IF;

    IF VAxisLLVel = LREAL#0.0 THEN
      MC_Halt_3(Execute:=FALSE);
      MC_Halt_3(Axis:=AxisLL,Execute:=TRUE,Deceleration:=Acc);
    ELSE
      MC_MoveVelocity_3(Execute:=FALSE);
      MC_MoveVelocity_3(Axis:=AxisLL,Execute:=TRUE,Velocity:=VAxisLLVel * RatioLimit,Acceleration:=Acc,Deceleration:=Acc);
    END_IF;

    IF VAxisLRVel = LREAL#0.0 THEN
      MC_Halt_4(Execute:=FALSE);
      MC_Halt_4(Axis:=AxisLR,Execute:=TRUE,Deceleration:=Acc);
    ELSE
      MC_MoveVelocity_4(Execute:=FALSE);
      MC_MoveVelocity_4(Axis:=AxisLR,Execute:=TRUE,Velocity:=VAxisLRVel * RatioLimit,Acceleration:=Acc,Deceleration:=Acc);
    END_IF;
  END_IF;

  MC_ReadActualVelocity_1(Axis:=AxisUL,Enable:=TRUE);
  MC_ReadActualVelocity_2(Axis:=AxisUR,Enable:=TRUE);
  MC_ReadActualVelocity_3(Axis:=AxisLL,Enable:=TRUE);
  MC_ReadActualVelocity_4(Axis:=AxisLR,Enable:=TRUE);

  AGVM_VelWheels2Cartesian_1(
    UL:=MC_ReadActualVelocity_1.Velocity,
    UR:=MC_ReadActualVelocity_2.Velocity,
    LL:=MC_ReadActualVelocity_3.Velocity,
    LR:=MC_ReadActualVelocity_4.Velocity);

  SHM_VelX:=AGVM_VelWheels2Cartesian_1.VelX;
  SHM_VelY:=AGVM_VelWheels2Cartesian_1.VelY;
  SHM_VelRZ:=AGVM_VelWheels2Cartesian_1.VelRZ;

  MC_ReadActualPosition_1(Axis:=AxisUL,Enable:=TRUE);
  MC_ReadActualPosition_2(Axis:=AxisUR,Enable:=TRUE);
  MC_ReadActualPosition_3(Axis:=AxisLL,Enable:=TRUE);
  MC_ReadActualPosition_4(Axis:=AxisLR,Enable:=TRUE);

  AGVM_VelWheels2Cartesian_2(
    UL:=MC_ReadActualPosition_1.Position - AxisULPos,
    UR:=MC_ReadActualPosition_2.Position - AxisURPos,
    LL:=MC_ReadActualPosition_3.Position - AxisLLPos,
    LR:=MC_ReadActualPosition_4.Position - AxisLRPos);

  SHM_PosX:=SHM_PosX + (AGVM_VelWheels2Cartesian_2.VelX * COS(SHM_PosRZ) - AGVM_VelWheels2Cartesian_2.VelY * SIN(SHM_PosRZ));
  SHM_PosY:=SHM_PosY + (AGVM_VelWheels2Cartesian_2.VelX * SIN(SHM_PosRZ) + AGVM_VelWheels2Cartesian_2.VelY * COS(SHM_PosRZ));
  SHM_PosRZ:=SHM_PosRZ + AGVM_VelWheels2Cartesian_2.VelRZ;

  AxisULPos:=MC_ReadActualPosition_1.Position;
  AxisURPos:=MC_ReadActualPosition_2.Position;
  AxisLLPos:=MC_ReadActualPosition_3.Position;
  AxisLRPos:=MC_ReadActualPosition_4.Position;

  MC_ReadAxisError_1(Axis:=AxisUL,Enable:=TRUE);
  AxisULErr:=MC_ReadAxisError_1.ErrorID;

  MC_ReadAxisError_1(Axis:=AxisUR,Enable:=TRUE);
  AxisURErr:=MC_ReadAxisError_1.ErrorID;

  MC_ReadAxisError_1(Axis:=AxisLL,Enable:=TRUE);
  AxisLLErr:=MC_ReadAxisError_1.ErrorID;

  MC_ReadAxisError_1(Axis:=AxisLR,Enable:=TRUE);
  AxisLRErr:=MC_ReadAxisError_1.ErrorID;
END_PROGRAM

PLCopen Motion with ROS2

The following links provide useful tutorials on using ROS2 with PLCopen Motion:

PLCopen Motion Sanity Check

The following section is applicable to:

../_images/target_generic1.png

Sanity Check #1: On-line S-Curve Algorithm Test

$ /opt/plcopen/online_scurve_test

Expected Output

[==========] Running 9 tests from 1 test suite.
[----------] Global test environment set-up.
[----------] 9 tests from ScurveTest
[ RUN      ] ScurveTest.Test1
Polynomial coeffs: a0 43.333340, a0 10.000000, a0 0.000000, a0 -5.625028, a0 2.109407, a0 -0.000009
Scurve profile: Tj2a 0.333333, Tj2b 0.333333, Td 1.333333
Scurve termination: sk_e -0.000000, vk_e 0.000002, ak_e -0.011247
[       OK ] ScurveTest.Test1 (1 ms)
[ RUN      ] ScurveTest.Test2
Polynomial coeffs: a0 0.819853, a0 3.933340, a0 5.000000, a0 -5.265315, a0 -0.676768, a0 1.109904
Scurve profile: Tj2a 0.666667, Tj2b 0.333333, Td 1.226667
Scurve termination: sk_e -0.000000, vk_e 0.000011, ak_e -0.032459
[       OK ] ScurveTest.Test2 (0 ms)
[ RUN      ] ScurveTest.Test3
Polynomial coeffs: a0 0.503677, a0 5.432135, a0 2.955000, a0 -7.379425, a0 3.228023, a0 0.143848
Scurve profile: Tj2a 0.530333, Tj2b 0.500000, Td 1.040094
Scurve termination: sk_e -0.000281, vk_e -0.000468, ak_e -0.004274
[       OK ] ScurveTest.Test3 (0 ms)
[ RUN      ] ScurveTest.Test4
Polynomial coeffs: a0 1.538060, a0 5.233840, a0 2.740000, a0 -1.969392, a0 -10.467462, a0 9.302328
Scurve profile: Tj2a 0.488227, Tj2b 0.238894, Td 0.727121
Scurve termination: sk_e -0.000362, vk_e -0.000241, ak_e -0.012134
[       OK ] ScurveTest.Test4 (0 ms)
[ RUN      ] ScurveTest.Test5
Polynomial coeffs: a0 43.090361, a0 9.993340, a0 0.000000, a0 -6.850631, a0 3.713191, a0 -0.260350
Scurve profile: Tj2a 0.333333, Tj2b 0.400000, Td 1.106001
Scurve termination: sk_e -0.000002, vk_e -0.000001, ak_e -0.000026
[       OK ] ScurveTest.Test5 (0 ms)
[ RUN      ] ScurveTest.Test6
Polynomial coeffs: a0 31.188293, a0 9.993340, a0 0.000000, a0 5.528178, a0 -1.977616, a0 -0.042063
Scurve profile: Tj2a 0.333333, Tj2b 0.266667, Td 1.273999
Scurve termination: sk_e -0.019986, vk_e -0.002014, ak_e 0.031347
[       OK ] ScurveTest.Test6 (0 ms)
[ RUN      ] ScurveTest.Test7
Polynomial coeffs: a0 1.532744, a0 14.421760, a0 -3.560000, a0 5.345304, a0 0.326130, a0 -1.078490
Scurve profile: Tj2a 0.570667, Tj2b 0.266667, Td 1.152981
Scurve termination: sk_e -0.019626, vk_e -0.001984, ak_e 0.044019
[       OK ] ScurveTest.Test7 (0 ms)
[ RUN      ] ScurveTest.Test8
Polynomial coeffs: a0 2.742635, a0 1.570000, a0 0.000000, a0 -6.130163, a0 6.063986, a0 -0.011689
Scurve profile: Tj2a 0.006280, Tj2b 0.006280, Td 0.506280
Scurve termination: sk_e -0.000000, vk_e 0.000001, ak_e -0.010276
[       OK ] ScurveTest.Test8 (0 ms)
[ RUN      ] ScurveTest.Test9
Polynomial coeffs: a0 2.443341, a0 6.753340, a0 5.000000, a0 -5.738710, a0 0.443898, a0 0.434246
Scurve profile: Tj2a 0.666667, Tj2b 0.333333, Td 1.508667
Scurve termination: sk_e -0.000000, vk_e 0.000009, ak_e -0.027303
[       OK ] ScurveTest.Test9 (0 ms)
[----------] 9 tests from ScurveTest (4 ms total)

[----------] Global test environment tear-down
[==========] 9 tests from 1 test suite ran. (4 ms total)
[  PASSED  ] 9 tests.

The tests verify the motor trajectory planning under different conditions.

The following section is applicable to:

../_images/target_generic1.png

Sanity Check #2: Trajectory Planner Test

$ /opt/plcopen/planner_test

Expected Output

[==========] Running 8 tests from 1 test suite.
[----------] Global test environment set-up.
[----------] 8 tests from PlannerTest
[ RUN      ] PlannerTest.VelocityUp
Scurve profile: Ta = 1.433333, Tv = 0.000000, Td = 0.000000, Tj1 = 0.333333, Tj2 = 0.000000
[       OK ] PlannerTest.VelocityUp (0 ms)
[ RUN      ] PlannerTest.VelocityDown
Scurve profile: Ta = 1.833333, Tv = 0.000000, Td = 0.000000, Tj1 = 0.333333, Tj2 = 0.000000
[       OK ] PlannerTest.VelocityDown (0 ms)
[ RUN      ] PlannerTest.Example_3_9
[       OK ] PlannerTest.Example_3_9 (0 ms)
[ RUN      ] PlannerTest.Example_3_10
[       OK ] PlannerTest.Example_3_10 (0 ms)
[ RUN      ] PlannerTest.Example_3_11
[       OK ] PlannerTest.Example_3_11 (0 ms)
[ RUN      ] PlannerTest.Example_3_12
[       OK ] PlannerTest.Example_3_12 (0 ms)
[ RUN      ] PlannerTest.Example_3_13
[       OK ] PlannerTest.Example_3_13 (0 ms)
[ RUN      ] PlannerTest.Test_Negative
[       OK ] PlannerTest.Test_Negative (0 ms)
[----------] 8 tests from PlannerTest (5 ms total)

[----------] Global test environment tear-down
[==========] 8 tests from 1 test suite ran. (5 ms total)
[  PASSED  ] 8 tests.

The tests verify the motor trajectory planning under different conditions.

The following section is applicable to:

../_images/target_generic1.png

Sanity Check #3: Function Block Test

$ /opt/plcopen/function_block_test

Expected Output

[==========] Running 8 tests from 1 test suite.
[----------] Global test environment set-up.
[----------] 8 tests from FunctionBlockTest
[ RUN      ] FunctionBlockTest.DemoRelative
Axis initialized.
Function block initialized.
Axis #1 powered on, axis_pos_ = 0.000000
FB test end. Delete axis and servo.
[       OK ] FunctionBlockTest.DemoRelative (2545 ms)
[ RUN      ] FunctionBlockTest.DemoVelocity
Axis initialized.
Function block initialized.
Axis #1 powered on, axis_pos_ = 0.000000
FB test end. Delete axis and servo.
[       OK ] FunctionBlockTest.DemoVelocity (565 ms)
[ RUN      ] FunctionBlockTest.MC_Stop
Axis initialized.
axis poweron, moveVel start
Axis #1 powered on, axis_pos_ = 0.000000
FB test end. Delete axis and servo.
[       OK ] FunctionBlockTest.MC_Stop (25000 ms)
[ RUN      ] FunctionBlockTest.MC_Halt
Axis initialized.
axis poweron, moveVel start
Axis #1 powered on, axis_pos_ = 0.000000
FB test end. Delete axis and servo.
[       OK ] FunctionBlockTest.MC_Halt (40000 ms)
[ RUN      ] FunctionBlockTest.MC_MoveAbsolute
Axis initialized.
axis poweron, moveVel start
Axis #1 powered on, axis_pos_ = 0.000000
FB test end. Delete axis and servo.
[       OK ] FunctionBlockTest.MC_MoveAbsolute (7000 ms)
[ RUN      ] FunctionBlockTest.MC_MoveRelative
Axis initialized.
axis poweron, moveVel start
Axis #1 powered on, axis_pos_ = 0.000000
FB test end. Delete axis and servo.
[       OK ] FunctionBlockTest.MC_MoveRelative (4000 ms)
[ RUN      ] FunctionBlockTest.MC_MoveAdditive
Axis initialized.
axis poweron, moveVel start
Axis #1 powered on, axis_pos_ = 0.000000
FB test end. Delete axis and servo.
[       OK ] FunctionBlockTest.MC_MoveAdditive (7000 ms)
[ RUN      ] FunctionBlockTest.MC_MoveVelocity
Axis initialized.
axis poweron, moveVel start
Axis #1 powered on, axis_pos_ = 0.000000
FB test end. Delete axis and servo.
[       OK ] FunctionBlockTest.MC_MoveVelocity (9000 ms)
[----------] 8 tests from FunctionBlockTest (95111 ms total)

[----------] Global test environment tear-down
[==========] 8 tests from 1 test suite ran. (95111 ms total)
[  PASSED  ] 8 tests.

The tests verify the function block execution result.

PLCopen Motion Recurring Testing

The following section is applicable to:

../_images/target_generic1.png

Recurring Test #1: Virtual Axis Motion Control Test

This test will repeatedly run the MC_MoveRelative function block for each axis of the axis array in a user defined real-time cycle. After the function block runs for a while, press crtl+c to stop the program. Performance data will be displayed during the run if the argument -o is enabled. Note that the performance depends on the chosen hardware and configurations. The program should be run with correct arguments. The argument list can be obtained by running /opt/plcopen/multi-axis-monitor --help. Check the example below to run 6 virtual axes with 1ms cycle time on CPU core 1 and print out the output. In the output, Sched_lat and Exec_time represent scheduler latency and execution time respectively.

$ sudo /opt/plcopen/multi-axis-monitor -i 1000 -a 1 -m 6 -o

Expected Output

Cycle-time: Set cycle time to 1000 (us)
CPU affinity: Set CPU affinity of rt thread to core 1
Axis number: Set axis number to 6
Output: print time statistic in 10000 us cycle.
    Dur(s)            Sched_lat(us)                     Exec_time(us)
        14 [     1.508,     2.276,    15.328] [     0.686,     1.417,    27.720]
1

https://www.canadianpackaging.com/automation/introducing-the-alpha-hsm-160845/

2(1,2)

https://plcopen.org/downloads/application-examples-plcopen-motion-control

3

https://en.wikipedia.org/wiki/Robot_Operating_System

4

https://en.wikipedia.org/wiki/Mecanum_wheel