MceActualPositions

0.3.0

general

Read actual positions and speed from a MotoLogix system.

This function reads the TCP speed and the various actual positions of each motion device in a MotoLogix system:

TCP position in the world frame
TCP position in the active user frame
axis position

Reading TCP speed requires parameter S2C1702=1

This function uses a state machine:

nSmActualPosition - getting actual position information from the MotoLogix system. It stores the position and speed information in arrays, see aPositions and aTcpSpeeds.

The position readings use different mechanisms.

Axis positions have their own channels and are updated cyclically. TCP positions share one communication channel and have to be red one after the other which leads to less frequent (but still fast) updates.

On a system with two robots this results in following read sequence:

TCP world (R1) + all axis positions
TCP user (R1) + all axis positions
TCP world (R2) + all axis positions
TCP user (R2) + all axis positions

A trace of this sequence (captured on a PLC running at 2ms) shows the different update frequencies:

sequence-normal — Normal sequence (R1+R2 system)

The read sequence can be temporarily locked to a specific robot/frame for fastest possible TCP speed and position updates, see nLockTarget.

sequence-lock-target — Locked target (R1+R2 system)

Usage

Each MotoLogix system needs its own MceActualPositions instance.

Create the (global) variables for the interface data. These are also used for connecting to an HMI.

stActualPositions : ARRAY [0..GVL.MLX_UBOUND] OF MceActualPositionsIO; // data for reading positions of a MotoLogix system

Map all relevant inputs (see MceActualPositionsIO) to your HMI and/or to other programs:

GVL.stActualPositions[0].bUseFeedbackPosition := ...;
GVL.stActualPositions[0].nLockTarget := ...;

Create the instances:

fbActualPositions : ARRAY [0..GVL.MLX_UBOUND] OF MceActualPositions;

Call the instances in a loop:

// function call
FOR i := 0 TO GVL.MLX_UBOUND DO
  fbActualPositions[i](
    io := GVL.stActualPositions[i],
    MLX := GVL.stMLX[i]);
END_FOR;

Map all relevant outputs (see MceActualPositionsIO) to your HMI and/or to other programs:

// this is just a portion of the relevant signals
... := GVL.stActualPositions[0].aTcpSpeeds[0];
... := GVL.stActualPositions[0].aPositions[0].aWorld;

Version history

0.3.0

24 Feb 2022 deGroot

Changes

Changed:

fix for axis positions

show full history

0.2.0

24 Feb 2022 deGroot

Changes

Changed:

fix for multiple robots

0.1.0

24 Feb 2022 deGroot

Changes

Added:

initial version

Overview

kind	name	type	default	comment
in_out	io	`MceActualPositionsIO`		interface data
in_out	MLX	`MLxData`		MotoLogix shared memory variable

Details

io

MceActualPositionsIO

Interface data for this function.

Check the data type for more information.

MLX

MLxData

The MotoLogix variable which acts as the shared memory for a MotoLogix system.

Check the YaskawaMLx library for more information.

Source code

Declarations

(*Read actual positions and speed*)
FUNCTION_BLOCK MceActualPositions

(*
 * -----------------------------------------------------------------------------
 * Name               : MceActualPositions
 * Version            : 0.3.0
 * Date               : 2022-02-24
 * Author             : deGroot
 * Family             : YaskawaMce
 * Organisation       : github.com/YaskawaEurope/mlx-examples
 * 
 * -----------------------------------------------------------------------------
 * Read actual positions and speed
 * -----------------------------------------------------------------------------
 *)

VAR_IN_OUT
  io : MceActualPositionsIO; (*interface data*)
  MLX : MLxData; (*MotoLogix shared memory variable*)
END_VAR

VAR
  bNewPosData : BOOL; (*new position data*)
  bLockWorld : BOOL; (*lock target: world frame*)
  bLockUserFrame : BOOL; (*lock target: user frame*)
  nRobotNumber : USINT; (*selected robot number*)
  nCoordFrame : USINT; (*selected coord frame*)
  i : DINT; (*index for FOR loops*)
  k : DINT; (*index for FOR loops*)
END_VAR

Logic

// -----------------------------------------------------------------------------
// common
// -----------------------------------------------------------------------------
// check for new position data
bNewPosData :=
  (MLX.HMIFeedbackData.CurrentRobotTCPCoordFrame = nCoordFrame);

  // -------------------------------------
// lock specific target: frame
  // -------------------------------------
bLockWorld :=
  (io.nLockTarget = 10) OR
  (io.nLockTarget = 20) OR
  (io.nLockTarget = 30) OR
  (io.nLockTarget = 40);

bLockUserFrame :=
  (io.nLockTarget = 12) OR
  (io.nLockTarget = 22) OR
  (io.nLockTarget = 32) OR
  (io.nLockTarget = 42);


// -----------------------------------------------------------------------------
// State machine 1: read actual TCP positions
// -----------------------------------------------------------------------------
CASE io.nSmActualPosition OF
  // -------------------------------------
  // idle
  // -------------------------------------
  0:
    IF MLX.Signals.MLXGatewayConnected THEN
      io.nSmActualPosition := 10;
    ELSE
      nRobotNumber := 0;
      // clear position data
      MEMUtils.MemSet(
        pbyBuffer := ADR(io.aPositions),
        byValue := 0,
        dwSize := SIZEOF(io.aPositions));

      // clear speed data
      MEMUtils.MemSet(
        pbyBuffer := ADR(io.aTcpSpeeds),
        byValue := 0,
        dwSize := SIZEOF(io.aTcpSpeeds));
    END_IF;

  // -------------------------------------
  // read TCP position in world
  // -------------------------------------
  10:
    nCoordFrame := 0;

    IF bNewPosData THEN
      // copy World positions
      MEMUtils.MemCpy(
        pbySrc := ADR(MLX.HMIFeedbackData.CurrentRobotTCPPosition),
        pbyDest := ADR(io.aPositions[nRobotNumber].aWorld),
        dwSize := SIZEOF(io.aPositions[nRobotNumber].aWorld));

      // copy TCP speed (requires S2C1702=1)
      io.aTcpSpeeds[nRobotNumber] :=
        MLX.HMIFeedbackData.CurrentRobotTCPPosition[7];

      IF NOT bLockWorld THEN
        // prepare next
        nCoordFrame := 2;
        io.nSmActualPosition := 20;
      END_IF;
    END_IF;

    IF NOT MLX.Signals.MLXGatewayConnected THEN
      io.nSmActualPosition := 0;
    END_IF;

  // -------------------------------------
  // read TCP position in active user frame
  // -------------------------------------
  20:
    nCoordFrame := 2;

    IF bNewPosData THEN
      // copy user frame positions
      MEMUtils.MemCpy(
        pbySrc := ADR(MLX.HMIFeedbackData.CurrentRobotTCPPosition),
        pbyDest := ADR(io.aPositions[nRobotNumber].aUser),
        dwSize := SIZEOF(io.aPositions[nRobotNumber].aUser));

      // copy TCP speed (requires S2C1702=1)
      io.aTcpSpeeds[nRobotNumber] :=
        MLX.HMIFeedbackData.CurrentRobotTCPPosition[7];

      IF NOT bLockUserFrame THEN
        // prepare next
        nCoordFrame := 0;
        nRobotNumber := nRobotNumber + 1;
        io.nSmActualPosition := 10;
      END_IF;
    END_IF;

    IF NOT MLX.Signals.MLXGatewayConnected THEN
      io.nSmActualPosition := 0;
    END_IF;

  ELSE
    io.nSmActualPosition := 0;
END_CASE;


// -----------------------------------------------------------------------------
// outputs
// -----------------------------------------------------------------------------
io.bPosUpdating := (io.nSmActualPosition <> 0);

// -------------------------------------
// lock specific target: robot
// -------------------------------------
CASE io.nLockTarget OF
  10,
  12:
    nRobotNumber := 0;
  20,
  22:
    nRobotNumber := 1;
  30,
  32:
    nRobotNumber := 2;
  40,
  42:
    nRobotNumber := 3;
END_CASE;

// -------------------------------------
// select feedback position
// -------------------------------------
IF io.bUseFeedbackPosition THEN
  nCoordFrame := nCoordFrame + 10;
END_IF;

// -------------------------------------
// set coord frame selection
// -------------------------------------
MLX.HMIFeedbackConfiguration.CurrentRobotJogCoordFrame := nCoordFrame;

// -------------------------------------
// set robot selection
// -------------------------------------
IF (nRobotNumber < 0) OR (nRobotNumber > (MLX.NumberOfRobots - 1)) THEN
  nRobotNumber := 0;
END_IF;
MLX.HMIFeedbackConfiguration.CurrentRobotNumber := nRobotNumber;


// -----------------------------------------------------------------------------
// read all axis positions
// -----------------------------------------------------------------------------
IF io.bPosUpdating THEN
  FOR i := 0 TO (MLX.NumberOfRobots - 1) DO
    FOR k := 0 TO 5 DO
      io.aPositions[i].aAxis[k] :=
        MLX.Robot[i].RobotAxes[k].FeedbackData.CommandedPosition;
    END_FOR;
  END_FOR;
END_IF;

Implementation

Snippet of the function call:

fbActualPositions : ARRAY[0..0] OF MceActualPositions;

fbActualPositions[0]( io := dummy,  MLX := dummy );