MceManualMotion

0.1.0
jog 

Handling manual motions for a MotoLogix system.

Whenever you need to adjust a position, teach a user frame, calibrate a tool or move the robot for maintenance some manual motions are required.

Features:

  • Jog axis (joints)
  • Jog TCP (cartesian motion)
  • Inching axis
  • Inching TCP
  • Handles the switching of MLX.JoggingMode.
  • Activates the selected Tool and User Frame for cartesian jog motions.

State machines:

Usage

Usually you only need one instance of this module – even if you have multiple MotoLogix systems and/or motion devices. By using a selector on the HMI you map the manual motion module to the desired system and motion device.

Switching to a different MotoLogix system is only allowed at manual motion standstill.

If you want to be able to perform manual motions simultaneous on multiple MotoLogix systems and/or motion devices you would need to create additional instances.

Create the (global) variable for the interface data. This is also used for connecting buttons or an HMI.

stManualMotion : MceManualMotionIO; // data for manual motion of MotoLogix motion devices

Create the instance:

fbManualMotion : MceManualMotion;

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

// this is just a portion of the relevant signals
GVL.stManualMotion.nMlxNumber := ...;
GVL.stManualMotion.nRobotNumber := ...;
GVL.stManualMotion.nOperatingMode := ...;
GVL.stManualMotion.bSystemReady := ...;
GVL.stManualMotion.bInchingMode := ...;
GVL.stManualMotion.aJogRobotAxisNeg := ...;
GVL.stManualMotion.aJogRobotAxisPos := ...;
GVL.stManualMotion.nJogType := ...;
GVL.stManualMotion.nCoordFrame := ...;

Call the instance:

// function call
fbManualMotion(
  io := GVL.stManualMotion,
  UserFrames := GVL.stUserFrames[<robot selection>],
  MLX := GVL.stMLX[<mlx selection>]);

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

// this is just a portion of the relevant signals
... := GVL.stManualMotion.bIdle;
... := GVL.stManualMotion.bBusy;
... := GVL.stManualMotion.bDone;
... := GVL.stManualMotion.aJogRobotAxisNegIndicator;
... := GVL.stManualMotion.aJogRobotAxisPosIndicator;

Version history

0.1.0

  deGroot

Changes

Added:
  • initial version

Overview

kindnametypedefaultcomment
in_outioMceManualMotionIOinterface data
in_outuserFramesMceUserFramescollection of user frames for this robot
in_outMLXMLxDataMotoLogix shared memory variable

Details

io

MceManualMotionIO

Interface data for this function.

Check the data type for more information.

userFrames

MceUserFrames

Collection of user frames for this robot.

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

(*Handling the manual motion.*)
FUNCTION_BLOCK MceManualMotion

(*
 * -----------------------------------------------------------------------------
 * Name               : MceManualMotion
 * Version            : 0.1.0
 * Date               : 2022-04-14
 * Author             : deGroot
 * Family             : YaskawaMce
 * Organisation       : github.com/YaskawaEurope/mlx-examples
 * 
 * -----------------------------------------------------------------------------
 * Handling the manual motion.
 * -----------------------------------------------------------------------------
 *)

VAR_IN_OUT
  io : MceManualMotionIO; (*interface data*)
  userFrames : MceUserFrames; (*collection of user frames for this robot*)
  MLX : MLxData; (*MotoLogix shared memory variable*)
END_VAR

VAR
  bJogPressed : BOOL; (*some jog button pressed*)
  bRunJog : BOOL; (*run jog commands*)
  bUseRotSpeed : BOOL; (*use rotational speed*)
  aDirections : ARRAY [0..5] OF SINT; (*jog motion per axis (0=standstill, 1=positive dir, -1=negative dir)*)
  bOsrRunJog : BOOL; (*rising edge*)
  bOperatingModeManual : BOOL; (*operating mode: manual*)
  bTcpSettingsNeedUpdate : BOOL; (*settings for TCP jog need to be updated*)
  bMlxNumberChanged : BOOL; (*value has changed*)
  bJogTypeChanged : BOOL; (*value has changed*)
  bRobotNumberChanged : BOOL; (*value has changed*)
  bCoordFrameChanged : BOOL; (*value has changed*)
  bToolChanged : BOOL; (*value has changed*)
  bUserFrameChanged : BOOL; (*value has changed*)
  bUpdateSettingsDone : BOOL; (*update settings: done*)
  bUpdateSettings : BOOL; (*start update settings*)
  aOneShots : ARRAY [0..0] OF BOOL; (*bits for creating one shot signals*)
  nCoordFrameSelected : USINT; (*selected value*)
  nUserFrameSelected : USINT; (*selected value*)
  nToolSelected : USINT; (*selected value*)
  nMlxNumberSelected : USINT; (*selected value*)
  nRobotNumberSelected : USINT; (*selected value*)
  nJogTypeSelected : USINT; (*selected value*)
  i : DINT; (*index for FOR loops*)
  fbJogAxis : MLxRobotJogAxes;
  fbJogTcp : MLxRobotJogTCP;
  fbSelectTool : MLxRobotSelectTool;
  fbSetUserFrame : MLxRobotSetUserFrame;
  fbMoveAxisRel : MLxRobotMoveAxisRelative;
  fbMoveLinRel : MLxRobotMoveLinearRelative;
  fbIdleTime : TON;
  fbCoastTime : TON;
END_VAR

VAR CONSTANT
  JOGTYPE_AXIS : USINT := 0; (*jog type: axis*)
  JOGTYPE_TCP : USINT := 1; (*jog type: TCP*)
  INCHING_DIST_AXIS : REAL := 0.1; (*inching distance for axis jog [deg]*)
  INCHING_DIST_TCP : REAL := 0.1; (*inching distance for cartesian jog [mm or deg]*)
  INCHING_SPEED : REAL := 10; (*inching speed [% rated]*)
  INCHING_BF : SINT := -1; (*BlendFactor for inching [-1 to 8]*)
  INCHING_ACC_DEC : REAL := 80; (*acceleration/deceleration for inching [% rated]*)
END_VAR

Logic

// -----------------------------------------------------------------------------
// init
// -----------------------------------------------------------------------------
io.bError := FALSE;

fbSelectTool.Enable := FALSE;
fbSetUserFrame.Enable := FALSE;
fbJogAxis.Enable := FALSE;
fbJogTcp.Enable := FALSE;
fbMoveAxisRel.Enable := FALSE;
fbMoveLinRel.Enable := FALSE;
fbIdleTime.IN := FALSE;
fbCoastTime.IN := FALSE;

MEMUtils.MemSet(
  pbyBuffer := ADR(fbJogAxis.Directions),
  byValue := 0,
  dwSize := SIZEOF(fbJogAxis.Directions));

MEMUtils.MemSet(
  pbyBuffer := ADR(fbJogTcp.Directions),
  byValue := 0,
  dwSize := SIZEOF(fbJogTcp.Directions));

MEMUtils.MemSet(
  pbyBuffer := ADR(fbMoveAxisRel.DeltaPosition),
  byValue := 0,
  dwSize := SIZEOF(fbMoveAxisRel.DeltaPosition));

MEMUtils.MemSet(
  pbyBuffer := ADR(fbMoveLinRel.DeltaPosition),
  byValue := 0,
  dwSize := SIZEOF(fbMoveLinRel.DeltaPosition));


// -----------------------------------------------------------------------------
// common
// -----------------------------------------------------------------------------
bOperatingModeManual := (io.nOperatingMode = GVL.OP_MANUAL);

// set jogging mode as default
MLX.JoggingMode := bOperatingModeManual;

// jog speed
io.fSpeed := LIMIT(0.1, io.fSpeed, 100);

// detect changes
bMlxNumberChanged := (io.nMlxNumber <> nMlxNumberSelected);
bRobotNumberChanged := (io.nRobotNumber <> nRobotNumberSelected);
bJogTypeChanged := (io.nJogType <> nJogTypeSelected);
bCoordFrameChanged := (io.nCoordFrame <> nCoordFrameSelected);
bToolChanged := (io.nTool <> nToolSelected);
bUserFrameChanged := (io.nUserFrame <> nUserFrameSelected);

IF bMlxNumberChanged OR
  bRobotNumberChanged OR
  bToolChanged OR
  bUserFrameChanged THEN

  bTcpSettingsNeedUpdate := TRUE;
END_IF;


// -----------------------------------------------------------------------------
// jog buttons to directions pos: 1, neg: -1
// -----------------------------------------------------------------------------
bJogPressed := FALSE;
bRunJog := FALSE;
bUseRotSpeed := FALSE;
FOR i := 0 TO 5 DO
  aDirections[i] := 0;
  IF io.aJogRobotAxisPos[i] OR io.aJogRobotAxisNeg[i] THEN
    bJogPressed := TRUE;
  END_IF;

  IF io.aJogRobotAxisPos[i] AND NOT io.aJogRobotAxisNeg[i] THEN
    aDirections[i] := 1;
    bRunJog := TRUE;
    IF (nJogTypeSelected = JOGTYPE_TCP) AND (i >= 3) THEN // TCP rotation?
      bUseRotSpeed := TRUE;
    END_IF;
  ELSIF NOT io.aJogRobotAxisPos[i] AND io.aJogRobotAxisNeg[i] THEN
    aDirections[i] := -1;
    bRunJog := TRUE;
    IF (nJogTypeSelected = JOGTYPE_TCP) AND (i >= 3) THEN // TCP rotation?
      bUseRotSpeed := TRUE;
    END_IF;
  END_IF;
END_FOR;

//block other axis when rotating the TCP
IF bUseRotSpeed THEN
  aDirections[0] := 0;
  aDirections[1] := 0;
  aDirections[2] := 0;
END_IF;

bOsrRunJog := bRunJog AND NOT aOneShots[0];
aOneShots[0] := bRunJog;


// -----------------------------------------------------------------------------
// state machine 1: manual motion
// -----------------------------------------------------------------------------
CASE io.nSmManualMotion OF
  // -------------------------------------
  // idle, not ready for jog
  // -------------------------------------
  0:
    IF io.bSystemReady AND bOperatingModeManual THEN
      io.nErrorCode := 0;
      bTcpSettingsNeedUpdate := TRUE;
      io.nSmManualMotion := 10;
    END_IF;

  // -------------------------------------
  // ready for jog
  // -------------------------------------
  10:
    nMlxNumberSelected := io.nMlxNumber;
    nRobotNumberSelected := io.nRobotNumber;
    nJogTypeSelected := io.nJogType;
    nCoordFrameSelected := io.nCoordFrame;

    IF bOsrRunJog THEN
      io.nSmManualMotion := 11;
    END_IF;

    IF NOT bOperatingModeManual THEN
      io.nSmManualMotion := 0;
    END_IF;

  // -------------------------------------
  // jog selection
  // -------------------------------------
  11:
    nMlxNumberSelected := io.nMlxNumber;
    nRobotNumberSelected := io.nRobotNumber;
    nJogTypeSelected := io.nJogType;
    nCoordFrameSelected := io.nCoordFrame;

    IF bRunJog THEN
      // jog axis
      IF (nJogTypeSelected = JOGTYPE_AXIS) THEN
        IF (io.bInchingMode) THEN
          io.nSmManualMotion := 25;
        ELSE
          io.nSmManualMotion := 20;
        END_IF;
      END_IF;

      // jog cartesian
      IF (nJogTypeSelected = JOGTYPE_TCP) THEN
        IF bTcpSettingsNeedUpdate THEN
          io.nSmManualMotion := 15;
        ELSE
          IF (io.bInchingMode) THEN
            io.nSmManualMotion := 35;
          ELSE
            io.nSmManualMotion := 30;
          END_IF;
        END_IF;
      END_IF;
    ELSE
      io.nSmManualMotion := 10;
    END_IF;

  // -------------------------------------
  // update TCP settings
  // -------------------------------------
  15:
    IF bUpdateSettingsDone THEN
      IF (io.bInchingMode) THEN
        io.nSmManualMotion := 35;
      ELSE
        io.nSmManualMotion := 30;
      END_IF;
    END_IF;

    IF bRobotNumberChanged OR bMlxNumberChanged THEN
      io.nErrorCode := 1000 + io.nSmManualMotion;
      io.nSmManualMotion := 99;
    END_IF;

  // -------------------------------------
  // jog axis with MLxRobotJogAxes
  // -------------------------------------
  20:
    FOR i := 0 TO 5 DO
      fbJogAxis.Directions[i] := aDirections[i];
    END_FOR;
    fbJogAxis.Enable := TRUE;

    IF fbJogAxis.Sts_EN AND fbJogAxis.Sts_DN THEN
      IF fbJogAxis.Sts_ER THEN
        io.nErrorCode := 1000 + io.nSmManualMotion;
        io.nSmManualMotion := 99;
      ELSE
        io.nSmManualMotion := 21;
      END_IF;
    END_IF;

    IF bRobotNumberChanged OR bMlxNumberChanged THEN
      io.nErrorCode := 1000 + io.nSmManualMotion;
      io.nSmManualMotion := 99;
    END_IF;

  // -------------------------------------
  // jog axis - idle time
  // -------------------------------------
  21:
    fbIdleTime.IN := TRUE;
    fbCoastTime.IN := TRUE;

    IF fbIdletime.Q THEN
      IF NOT bRunJog OR
        NOT bOperatingModeManual OR
        bJogTypeChanged THEN
        io.nSmManualMotion := 22;
      ELSE
        io.nSmManualMotion := 11;
      END_IF;
    END_IF;

  // -------------------------------------
  // jog axis - coasting
  // -------------------------------------
  22:
    fbCoastTime.IN := TRUE;
    IF fbCoastTime.Q THEN
      io.nSmManualMotion := 50;
    END_IF;

  // -------------------------------------
  // inch axis with MLxRobotMoveAxisRelative
  // -------------------------------------
  25:
    FOR i := 0 TO 5 DO
      fbMoveAxisRel.DeltaPosition[i] :=
        (INCHING_DIST_AXIS * SINT_TO_REAL(aDirections[i]));
    END_FOR;

    MLX.JoggingMode := FALSE;
    fbMoveAxisRel.Enable := TRUE;

    IF fbMoveAxisRel.Sts_EN AND fbMoveAxisRel.Sts_DN THEN
      IF fbMoveAxisRel.Sts_ER THEN
        io.nErrorCode := 1000 + io.nSmManualMotion;
        io.nSmManualMotion := 99;
      END_IF;

      IF fbMoveAxisRel.Sts_PC THEN
        io.nSmManualMotion := 50;
      END_IF;
    END_IF;

    IF bRobotNumberChanged OR bMlxNumberChanged THEN
      io.nErrorCode := 1000 + io.nSmManualMotion;
      io.nSmManualMotion := 99;
    END_IF;

  // -------------------------------------
  // jog TCP with MLxRobotJogTCP
  // -------------------------------------
  30:
    FOR i := 0 TO 5 DO
      fbJogTcp.Directions[i] := aDirections[i];
    END_FOR;
    fbJogTcp.Enable := TRUE;

    IF fbJogTcp.Sts_EN AND fbJogTcp.Sts_DN THEN
      IF fbJogTcp.Sts_ER THEN
        io.nErrorCode := 1000 + io.nSmManualMotion;
        io.nSmManualMotion := 99;
      ELSE
        io.nSmManualMotion := 31;
      END_IF;
    END_IF;

    IF bRobotNumberChanged OR bMlxNumberChanged THEN
      io.nErrorCode := 1000 + io.nSmManualMotion;
      io.nSmManualMotion := 99;
    END_IF;

  // -------------------------------------
  // jog TCP - idle time
  // -------------------------------------
  31:
    fbIdleTime.IN := TRUE;
    fbCoastTime.IN := TRUE;

    IF fbIdletime.Q THEN
      IF NOT bRunJog OR
        NOT bOperatingModeManual OR
        bJogTypeChanged OR
        bCoordFrameChanged OR
        bToolChanged THEN
        io.nSmManualMotion := 32;
      ELSE
        io.nSmManualMotion := 11;
      END_IF;
    END_IF;

  // -------------------------------------
  // jog TCP - coasting
  // -------------------------------------
  32:
    fbCoastTime.IN := TRUE;
    IF fbCoastTime.Q THEN
      io.nSmManualMotion := 50;
    END_IF;

  // -------------------------------------
  // inch TCP with MLxRobotMoveLinearRelative
  // -------------------------------------
  35:
    FOR i := 0 TO 5 DO
      fbMoveLinRel.DeltaPosition.TCPPosition[i] :=
        (INCHING_DIST_TCP * SINT_TO_REAL(aDirections[i]));
    END_FOR;

    MLX.JoggingMode := FALSE;
    fbMoveLinRel.Enable := TRUE;

    IF fbMoveLinRel.Sts_EN AND fbMoveLinRel.Sts_DN THEN
      IF fbMoveLinRel.Sts_ER THEN
        io.nErrorCode := 1000 + io.nSmManualMotion;
        io.nSmManualMotion := 99;
      END_IF;

      IF fbMoveLinRel.Sts_PC THEN
        io.nSmManualMotion := 50;
      END_IF;
    END_IF;

    IF bRobotNumberChanged OR bMlxNumberChanged THEN
      io.nErrorCode := 1000 + io.nSmManualMotion;
      io.nSmManualMotion := 99;
    END_IF;

  // -------------------------------------
  // done
  // -------------------------------------
  50:
    IF NOT bJogPressed THEN
      IF io.bSystemReady AND bOperatingModeManual THEN
        io.nSmManualMotion := 10;
      ELSE
        io.nSmManualMotion := 0;
      END_IF;
    END_IF;

  // -------------------------------------
  // state machine error
  // -------------------------------------
  99:
    io.bError := TRUE;
    IF NOT bRunJog THEN
      io.nSmManualMotion := 0;
    END_IF;

  ELSE
    io.nSmManualMotion := 0;
END_CASE;


// -----------------------------------------------------------------------------
// reset
// -----------------------------------------------------------------------------
IF NOT io.bsystemReady THEN
  io.nSmManualMotion := 0;
END_IF;


// -----------------------------------------------------------------------------
// outputs
// -----------------------------------------------------------------------------
io.bIdle := (io.nSmManualMotion = 0) OR (io.nSmManualMotion = 10);
bUpdateSettings := (io.nSmManualMotion = 15);
io.bDone := (io.nSmManualMotion = 50);
io.bBusy := (io.nSmManualMotion > 10) AND (io.nSmManualMotion < 50);
io.bCoasting := (io.nSmManualMotion = 22) OR (io.nSmManualMotion = 32) ;

FOR i := 0 TO 5 DO
  IF io.bBusy THEN
    io.aJogRobotAxisNegIndicator[i] := io.aJogRobotAxisNeg[i];
    io.aJogRobotAxisPosIndicator[i] := io.aJogRobotAxisPos[i];
  END_IF;

  IF NOT io.bBusy AND NOT io.bCoasting THEN
    io.aJogRobotAxisNegIndicator[i] := FALSE;
    io.aJogRobotAxisPosIndicator[i] := FALSE;
  END_IF
END_FOR;


// -----------------------------------------------------------------------------
// state machine 2: settings
// -----------------------------------------------------------------------------
CASE io.nSmSettings OF
  // -------------------------------------
  // idle
  // -------------------------------------
  0:
    IF bUpdateSettings OR
       (bTcpSettingsNeedUpdate AND io.bSystemReady AND bOperatingModeManual AND io.bIdle) THEN
      io.nErrorCode := 0;
      io.nSmSettings := 10;
    END_IF;

  // -------------------------------------
  // select tool
  // -------------------------------------
  10:
    // init
    MEMUtils.MemSet(
    pbyBuffer := ADR(fbSelectTool.ToolData),
    byValue := 0,
    dwSize := SIZEOF(fbSelectTool.ToolData));

    fbSelectTool.RobotNumber := io.nRobotNumber;
    fbSelectTool.ToolNumber := io.nTool;
    fbSelectTool.Enable := TRUE;
    IF fbSelectTool.Sts_EN AND fbSelectTool.Sts_DN THEN
      IF fbSelectTool.Sts_ER THEN
        io.nErrorCode := 2000 + io.nSmSettings;
        io.nSmSettings := 99;
      ELSE
        nToolSelected := io.nTool;
        io.nSmSettings := 20;
      END_IF;
    END_IF;

  // -------------------------------------
  // set user frame
  // -------------------------------------
  20:
    // init
    MEMUtils.MemSet(
    pbyBuffer := ADR(fbSetUserFrame.UserFrameData),
    byValue := 0,
    dwSize := SIZEOF(fbSetUserFrame.UserFrameData));

    fbSetUserFrame.UserFrameData.CoordFrame :=
      userFrames.aFrames[io.nUserFrame].aVector;

    fbSetUserFrame.RobotNumber := io.nRobotNumber;
    fbSetUserFrame.UserFrameNumber := io.nUserFrame;
    fbSetUserFrame.Enable := TRUE;

    IF fbSetUserFrame.Sts_EN AND fbSetUserFrame.Sts_DN THEN
      IF fbSetUserFrame.Sts_ER THEN
        io.nErrorCode := 2000 + io.nSmSettings;
        io.nSmSettings := 99;
      ELSE
        nUserFrameSelected := io.nUserFrame;
        io.nSmSettings := 30;
      END_IF;
    END_IF;

  // -------------------------------------
  // zero-distance motion with MLxRobotMoveAxisRelative (updates actual position)
  // -------------------------------------
  30:
    // init
    MEMUtils.MemSet(
    pbyBuffer := ADR(fbMoveAxisRel.DeltaPosition),
    byValue := 0,
    dwSize := SIZEOF(fbMoveAxisRel.DeltaPosition));

    MLX.JoggingMode := FALSE;
    fbMoveAxisRel.Enable := TRUE;

    IF fbMoveAxisRel.Sts_EN AND fbMoveAxisRel.Sts_DN THEN
      IF fbMoveAxisRel.Sts_ER THEN
        io.nErrorCode := 2000 + io.nSmSettings;
        io.nSmSettings := 99;
      END_IF;

      IF fbMoveAxisRel.Sts_PC THEN
        nUserFrameSelected := io.nUserFrame;
        io.nSmSettings := 50;
      END_IF;
    END_IF;

  // -------------------------------------
  // done
  // -------------------------------------
  50:
    bTcpSettingsNeedUpdate := FALSE;
    IF NOT bUpdateSettings THEN
      io.nSmSettings := 0;
    END_IF;

  // -------------------------------------
  // state machine error
  // -------------------------------------
  99:
    io.bError := TRUE;
    io.nSmSettings := 0;

  ELSE
    io.nSmSettings := 0;
END_CASE;


// -----------------------------------------------------------------------------
// reset
// -----------------------------------------------------------------------------
IF NOT io.bsystemReady THEN
  io.nSmSettings := 0;
END_IF;


// -----------------------------------------------------------------------------
// outputs
// -----------------------------------------------------------------------------
bUpdateSettingsDone := (io.nSmSettings = 50);


// -----------------------------------------------------------------------------
// FB calls
// -----------------------------------------------------------------------------
fbIdleTime(PT:=io.tIdleTime);
fbCoastTime(PT:=io.tCoastTime);
fbSelectTool( MLX := MLX );
fbSetUserFrame( MLX := MLX );

fbJogAxis(
  RobotNumber := nRobotNumberSelected,
  Speed := io.fSpeed,
  MLX:= MLX);

fbJogTcp(
  RobotNumber := nRobotNumberSelected,
  Speed := io.fSpeed,
  UseRotationalSpeed := bUseRotSpeed,
  SpeedUnits := 0,
  CoordFrame := nCoordFrameSelected,
  MLX := MLX);

fbMoveAxisRel(
  RobotNumber := nRobotNumberSelected,
  BlendFactor := INCHING_BF,
  BlendType := 0,
  Speed := INCHING_SPEED,
  Acceleration := INCHING_ACC_DEC,
  Deceleration := INCHING_ACC_DEC,
  MLX := MLX );

fbMoveLinRel(
  RobotNumber := nRobotNumberSelected,
  BlendFactor := INCHING_BF,
  BlendType := 0,
  Speed := INCHING_SPEED,
  UseRotationalSpeed := bUseRotSpeed,
  SpeedUnits := 0,
  Acceleration := INCHING_ACC_DEC,
  Deceleration := INCHING_ACC_DEC,
  CoordFrame := nCoordFrameSelected,
  MLX := MLX);

Implementation

Snippet of the function call:
fbManualMotion : ARRAY[0..0] OF MceManualMotion;

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

Pages built with Hugo - 23 Apr 2024 11:54 CEST