McePosTable

(*Performed a trajectory stored in a PosTable*)
FUNCTION_BLOCK McePosTable

(*
 * -----------------------------------------------------------------------------
 * Name               : McePosTable
 * Version            : 0.7.1
 * Date               : 2025-06-06
 * Author             : Rioual
 * Family             : YaskawaMce
 * Organisation       : github.com/YaskawaEurope/mlx-examples
 * 
 * -----------------------------------------------------------------------------
 * Performed a trajectory stored in a PosTable
 * -----------------------------------------------------------------------------
 *)

VAR_IN_OUT
  io : McePosTableIO; (*McePosTable IO variables*)
  MLX : MLxData; (*MotoLogix shared memory variable*)
  posTable : McePosTableData; (*trajectory to be processed by PosTable*)
  userFrames : MceUserFrames; (*user frame data*)
  tools : MceToolData; (*tool data*)
END_VAR

VAR
  aMoveCmd : ARRAY [0..QA_MAX] OF MceCmdStatus; (*status signals of mapped move instruction*)
  fbRobotMoveAxisAbsolute : ARRAY [0..QA_MAX] OF MLxRobotMoveAxisAbsolute; (*move to target absolute axis position using PTP motion*)
  fbRobotMoveLinearAbsolute : ARRAY [0..QA_MAX] OF MLxRobotMoveLinearAbsolute; (*move to target TCP position in straight-line motion*)
  fbRobotSelectTool : ARRAY [0..QA_MAX] OF MLxRobotSelectTool; (*select the tool*)
  fbRobotSetUserFrame : ARRAY [0..QA_MAX] OF MLxRobotSetUserFrame; (*set the active User Frame for the Robot*)
  stStateMonitoringData : MceStateMonitoringData; (*monitor the elapsed time of a state*)
  aUseMoveType : ARRAY [0..QA_MAX] OF SINT; (*use linear motion for this mapped move*)
  nInstanceNumber : ARRAY [0..QA_MAX] OF INT; (*mapping of the instances; value in nInstanceNumber[0] tells the instance of the active motion.*)
  nQA : INT; (*allowed Queueing Amount; number of motions to queue (for smooth motion)*)
  nUserFrameNr : DINT;
  nToolNr : DINT;
  nPosTableSize : INT; (*number of entries*)
  nLastEntryIndex : DINT; (*last not skipped entry index of posTable (last move or last not skipped entry)*)
  aEnable : ARRAY [0..QA_MAX] OF BOOL; (*enable mapped move instruction*)
  aChangeTool : ARRAY [0..QA_MAX] OF BOOL; (*change the tool for this mapped move*)
  aChangeUserFrame : ARRAY [0..QA_MAX] OF BOOL; (*change the user frame for this mapped move*)
  aOneShots : ARRAY [0..QA_MAX+6] OF BOOL; (*rising edge signals*)
  aOsrEnable : ARRAY [0..QA_MAX] OF BOOL; (*rising edge*)
  bLastEntry : BOOL; (*last entry in process*)
  bFirstMove : BOOL; (*calc first move after system was reset*)
  bDefaultActionDone : BOOL; (*default action at standstill done*)
  bOsrRun : BOOL; (*rising edge*)
  bOsfRun : BOOL; (*falling edge*)
  bOsrStep : BOOL; (*rising edge*)
  bOsrSystemReady : BOOL; (*rising edge*)
  bOsrReset : BOOL; (*rising edge*)
  bWaiting : BOOL; (*wait for bStep*)
  bStopEntry : BOOL; (*entry with robot standstill (Action > 0) in process*)
  bWaitBeforeNextEntry : BOOL; (*next valid entry with conditions*)
  bWaitForCondition : BOOL; (*conditions for the next valid entry are not satisfied*)
  bOsrRecalcQA : BOOL; (*rising edge*)
  bError : BOOL; (*active when any MotoLogix FB returned an error*)
  bLastEntryInProcess : BOOL; (*last motion of the PosTable is running*)
  bOsfLastMoveInProcess : BOOL; (*falling edge*)
  bAllowQueuing : BOOL; (*allow following motions to be queued*)
  bStopAfterLastEntry : BOOL; (*stop PosTable after the last entry*)
  stConditionResult : McePosTableConditions; (*state if the next motion condition are satisfied*)
END_VAR

VAR_TEMP
  i : DINT; (*index for FOR loops*)
  bCustomActionDone : BOOL; (*check if input bCustomActionDone correspond to the right custom ActionID*)
  bUserFrameDone : BOOL; (*user frame has been loaded by MLxRobotSetUserFrame*)
  bToolDone : BOOL; (*tool has been loaded by MLxRobotSelectTool*)
  bUpdateQA : BOOL; (*QA update condition*)
  nNextIndex : INT; (*next valid index of PosTable*)
  nNextValidLoadIndex : INT; (*next valid loadIndex of PosTable*)
END_VAR

VAR CONSTANT
  QA_MAX : INT := LIMIT(1, GVL.NR_OF_INSTANCES - 1, 20); (*max Queueing Amount; max number of motions which can be queued*)
  RESERVED_ACTIONID_UBOUND : SINT := 19; (*actionID lower or equal to this value are reserved for default ActionID*)
  PT_MODE_0 : SINT := 0; (*PosTable in continuous mode*)
  PT_MODE_2 : SINT := 2; (*PosTable in step-by-step mode*)
  ACTIONID_NO_ACTION : SINT := 0; (*no action*)
  ACTIONID_WAIT_ONE_SCAN : SINT := 1; (*wait one PLC scan*)
  ACTIONID_WAIT_STEP : SINT := 2; (*wait for rising edge*)
END_VAR

// -----------------------------------------------------------------------------
// common
// -----------------------------------------------------------------------------
nPosTableSize := UINT_TO_INT(SIZEOF(posTable) / SIZEOF(posTable.stEntry[0]));
io.nIndex := LIMIT(0, io.nIndex, nPosTableSize - 1);
io.nLoadIndex := LIMIT(0, io.nLoadIndex, nPosTableSize - 1);

bOsrStep := io.bStep AND NOT aOneShots[0];
aOneShots[0] := io.bStep;
bOsrSystemReady := io.bSystemReady AND NOT aOneShots[1];
aOneShots[1] := io.bSystemReady;
bOsrRecalcQA := io.bRecalcQA AND NOT aOneShots[2];
aOneShots[2] := io.bRecalcQA;
bOsfRun := NOT io.bRun AND aOneShots[3];
aOneShots[3] := io.bRun;

// Search for the last entry of PosTable (either bLastEntry or last not skipped entry)
nLastEntryIndex := 0;
FOR i := 0 TO nPosTableSize - 1 DO
  IF posTable.stEntry[i].bLastEntry OR NOT posTable.stEntry[i].bSkipEntry THEN
    nLastEntryIndex := i;
    IF posTable.stEntry[i].bLastEntry THEN
      EXIT;
    END_IF;
  END_IF;
END_FOR;

bLastEntryInProcess := posTable.stEntry[io.nIndex].bLastEntry OR (io.nIndex = nLastEntryIndex);
bOsfLastMoveInProcess := NOT bLastEntryInProcess AND aOneShots[4];
aOneShots[4] := bLastEntryInProcess;

bOsrRun := io.bRun AND NOT aOneShots[5];
aOneShots[5] := io.bRun;

// Count the number of cycles
IF bOsfLastMoveInProcess AND io.bBusy THEN
  io.nCycleNr := io.nCycleNr + 1;
END_IF;

bStopAfterLastEntry := ((io.nCycleNr + 1) >= io.nNrOfCycles) AND NOT (io.nNrOfCycles = 0);

bUpdateQA := FALSE;
IF io.bRun AND (io.nMode = PT_MODE_0) AND (nQA < QA_MAX) THEN
  nNextValidLoadIndex := McePosTableFindEntry(io.nLoadIndex, 1, nPosTableSize, NOT bStopAfterLastEntry, posTable);
  IF nNextValidLoadIndex >= 0 AND posTable.stEntry[io.nLoadIndex].nActionID = 0 THEN
    stConditionResult := McePosTableCheckConditions(
      index := nNextValidLoadIndex,
      posTableSize := nPosTableSize,
      conditions := io.aConditions,
      posTable := posTable);
    bUpdateQA := stConditionResult.bConditionsOk;
  END_IF;
END_IF;

bWaiting := FALSE;

bAllowQueuing := (io.nMode = PT_MODE_0) AND io.bRun;

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

// clear Enable signals but keep signals enabled if an error occurs
IF io.nSmPosTable <> 99 THEN
  MEMUtils.MemSet(pbyBuffer := ADR(aEnable), byValue :=  0, dwSize :=  SIZEOF(aEnable));
END_IF;
io.bError := FALSE;

// Update QA
bUpdateQA := bUpdateQA OR bOsrRecalcQA OR (bOsrSystemReady AND io.nSmPosTable >= 10 AND io.nSmPosTable < 30);
IF bUpdateQA THEN
  bFirstMove := bFirstMove OR bOsrSystemReady OR bOsrRun; // only if first move after standstill
  McePosTableRecalcQA(allowQueuing := bAllowQueuing,
                    firstMove := bFirstMove,
                    stopAfterLastEntry := bStopAfterLastEntry,
                    loadIndex := io.nLoadIndex,
                    conditions := io.aConditions,
                    posTable := posTable,
                    QA := nQA,
                    lastEntry := bLastEntry,
                    stopEntry := bStopEntry,
                    waitBeforeNextEntry := bWaitBeforeNextEntry,
                    index := io.nIndex
                    );
END_IF;


// -----------------------------------------------------------------------------
// state machine 1: Motion
// -----------------------------------------------------------------------------
CASE io.nSmPosTable OF
  // -------------------------------------
  // state 0 - idle, not ready for start
  // -------------------------------------
  0:
    IF bOsfRun THEN
      io.nSmPosTable := 60;
    ELSIF io.bSystemReady THEN
      io.nErrorCode := 0;
      io.nSmPosTable := 1;
    END_IF;

  // -------------------------------------
  // state 1 - idle, ready for start
  // -------------------------------------
  1:
    IF NOT io.bSystemReady THEN
      io.nSmPosTable := 0;
    ELSIF bOsfRun THEN
      io.nSmPosTable := 60;
    ELSIF io.bSystemReady THEN

      bWaiting := (io.nMode = PT_MODE_2) AND io.bRun;
      bFirstMove := TRUE;

      IF ((io.nMode <> PT_MODE_2 AND io.bRun) OR (io.nMode = PT_MODE_2 AND bOsrStep AND io.bRun)) THEN
        io.nLoadIndex := io.nIndex;
        McePosTableUpdateIndex( allowQueuing := bAllowQueuing,
                                firstMove := bFirstMove,
                                stopAfterLastEntry := bStopAfterLastEntry,
                                loadIndex := io.nLoadIndex,
                                conditions := io.aConditions,
                                index := io.nIndex,
                                QA := nQA,
                                lastEntry := bLastEntry,
                                stopEntry := bStopEntry,
                                waitBeforeNextEntry := bWaitBeforeNextEntry,
                                posTable := posTable);
        IF io.nIndex < 0 THEN
          io.nIndex := 0;
          io.nSmPosTable := 50;
        ELSE
          stConditionResult := McePosTableCheckConditions(
            index := io.nIndex,
            posTableSize := nPosTableSize,
            conditions := io.aConditions,
            posTable := posTable);

          IF NOT stConditionResult.bConditionsOk THEN
            io.nSmPosTable := 5;
          ELSE
            io.nSmPosTable := 10;
          END_IF;
        END_IF;
      END_IF;
    END_IF;

  // -------------------------------------
  // state 5 - wait for conditions
  // -------------------------------------
  5:
    IF NOT io.bRun THEN
      io.nSmPosTable := 50;
    ELSIF io.bSystemReady THEN
      stConditionResult := McePosTableCheckConditions(
        index := io.nIndex,
        posTableSize := nPosTableSize,
        conditions := io.aConditions,
        posTable := posTable);

      IF stConditionResult.bConditionsOk AND ((io.nMode <> PT_MODE_2 AND io.bRun) OR (bOsrStep OR (nQA > 0))) THEN
        bWaitBeforeNextEntry := FALSE;
        McePosTableRecalcQA(allowQueuing := bAllowQueuing,
          firstMove := bFirstMove,
          stopAfterLastEntry := bStopAfterLastEntry,
          loadIndex := io.nLoadIndex,
          conditions := io.aConditions,
          posTable := posTable,
          QA := nQA,
          lastEntry := bLastEntry,
          stopEntry := bStopEntry,
          waitBeforeNextEntry := bWaitBeforeNextEntry,
          index := io.nIndex
          );
        io.nSmPosTable := 10;
      END_IF;
    END_IF;

  // -------------------------------------
  // state 10..29 - handle motion commands
  // -------------------------------------
  10..29:
    FOR i := 0 TO QA_MAX DO
      nInstanceNumber[i] := io.nSmPosTable + DINT_TO_INT(i) - 10;
      IF nInstanceNumber[i] > QA_MAX THEN
        nInstanceNumber[i] := nInstanceNumber[i] - (QA_MAX + 1);
      END_IF;
    END_FOR;

    IF NOT io.bRun AND ((io.nAbortingMode = 1) OR NOT io.bSystemReady) THEN
      io.nSmPosTable := 50;
    ELSIF io.bSystemReady THEN
      // start this motion
      aEnable[nInstanceNumber[0]] := TRUE;

      // queue next motions
      FOR i := 1 TO QA_MAX DO
        aEnable[nInstanceNumber[i]] := (aMoveCmd[nInstanceNumber[i-1]].bSts_EN AND aMoveCmd[nInstanceNumber[i-1]].bSts_DN) AND (nQA >= i);
      END_FOR;

      // motion completed
      IF (aMoveCmd[nInstanceNumber[0]].bSts_EN AND aMoveCmd[nInstanceNumber[0]].bSts_PC) THEN
        IF NOT io.bRun AND (nQA = 0) THEN
          io.nSmPosTable := 50;
        ELSIF bStopEntry THEN
          io.nSmPosTable := 30;
        ELSIF bLastEntry THEN
          io.nSmPosTable := 40;
        ELSE
          bWaiting := (io.nMode = PT_MODE_2);
          IF (io.nMode <> PT_MODE_2 AND io.bRun) OR (bOsrStep OR (nQA > 0)) THEN
            aEnable[nInstanceNumber[0]] := FALSE;
            IF bWaitBeforeNextEntry THEN
              io.nSmPosTable := 5;
            ELSE
              io.nSmPosTable := nInstanceNumber[1] + 10;
            END_IF;
            McePosTableUpdateIndex( allowQueuing := bAllowQueuing,
                                    firstMove := bFirstMove,
                                    stopAfterLastEntry := bStopAfterLastEntry,
                                    loadIndex := io.nLoadIndex,
                                    conditions := io.aConditions,
                                    index := io.nIndex,
                                    QA := nQA,
                                    lastEntry := bLastEntry,
                                    stopEntry := bStopEntry,
                                    waitBeforeNextEntry := bWaitBeforeNextEntry,
                                    posTable := posTable);
          END_IF;
        END_IF;
      END_IF;
    END_IF;

  // -------------------------------------
  // state 30 - waiting before performing action at standstill
  // -------------------------------------
  30:
    IF NOT io.bRun THEN
      io.nSmPosTable := 50;
    ELSIF io.bSystemReady THEN
        bWaiting := (io.nMode = PT_MODE_2);
        IF (io.nMode <> PT_MODE_2) OR bOsrStep THEN
            io.nSmPosTable := 31;
        END_IF;
    END_IF;

  // -------------------------------------
  // state 31 - perform action at standstill
  // -------------------------------------
  31:
    IF NOT io.bRun THEN
      io.nSmPosTable := 50;
    ELSIF io.bSystemReady THEN
      bWaiting := (bDefaultActionDone OR io.bCustomActionDone) AND (io.nMode = PT_MODE_2);
      bCustomActionDone := io.bCustomActionDone AND (io.nActionID > RESERVED_ACTIONID_UBOUND);
      IF bDefaultActionDone OR bCustomActionDone THEN
        io.nSmPosTable := 32;
      END_IF;
    END_IF;

  // -------------------------------------
  // state 32 - waiting after performing action
  // -------------------------------------
  32:
    IF NOT io.bRun THEN
      io.nSmPosTable := 50;
    ELSIF io.bSystemReady THEN
      bWaiting := (io.nMode = PT_MODE_2);
      IF (io.nMode <> PT_MODE_2) OR bOsrStep THEN
        IF bLastEntry AND (io.nNrOfCycles - io.nCycleNr <= 1) THEN
          io.nSmPosTable := 40;
        ELSE
          IF bWaitBeforeNextEntry THEN
            io.nSmPosTable := 5;
          ELSE
            io.nSmPosTable := 10;
          END_IF;
          McePosTableUpdateIndex( allowQueuing := bAllowQueuing,
                                  firstMove := bFirstMove,
                                  stopAfterLastEntry := bStopAfterLastEntry,
                                  loadIndex := io.nLoadIndex,
                                  conditions := io.aConditions,
                                  index := io.nIndex,
                                  QA := nQA,
                                  lastEntry := bLastEntry,
                                  stopEntry := bStopEntry,
                                  waitBeforeNextEntry := bWaitBeforeNextEntry,
                                  posTable := posTable);
        END_IF;
      END_IF;
    END_IF;

  // -------------------------------------
  // state 40 - cycle completed
  // -------------------------------------
  40:
    io.nCycleNr := io.nCycleNr + 1;

    IF NOT io.bRun THEN
          io.nSmPosTable := 50;
    ELSIF ((io.nCycleNr < io.nNrOfCycles) OR (io.nNrOfCycles = 0)) AND (io.nMode <> PT_MODE_2) THEN
      IF bWaitBeforeNextEntry THEN
        io.nSmPosTable := 5;
      ELSE
        io.nSmPosTable := 10;
      END_IF;
      McePosTableUpdateIndex( allowQueuing := bAllowQueuing,
                              firstMove := bFirstMove,
                              stopAfterLastEntry := FALSE,
                              loadIndex := io.nLoadIndex,
                              conditions := io.aConditions,
                              index := io.nIndex,
                              QA := nQA,
                              lastEntry := bLastEntry,
                              stopEntry := bStopEntry,
                              waitBeforeNextEntry := bWaitBeforeNextEntry,
                              posTable := posTable);
    ELSE
      io.nSmPosTable := 50;
    END_IF;

  // -------------------------------------
  // state 50 - wait at the end of cycle
  // -------------------------------------
  50:
    IF NOT io.bRun THEN
      io.nSmPosTable := 60;
    END_IF;

  // -------------------------------------
  // state 60 - reset cycle
  // -------------------------------------
  60:
    io.nIndex := 0;
    io.nLoadIndex := 0;
    io.nCycleNr := 0;

    io.nSmPosTable := 0;

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

    IF NOT io.bRun THEN
      // Reset FB
      MEMUtils.MemSet(pbyBuffer := ADR(aEnable), byValue :=  0, dwSize :=  SIZEOF(aEnable));
      io.nSmPosTable := 60;
    END_IF;
END_CASE;


// -----------------------------------------------------------------------------
// actions
// -----------------------------------------------------------------------------
io.bActionStart := (io.nSmPosTable = 31);
io.nActionID := posTable.stEntry[io.nIndex].nActionID;

IF io.bActionStart THEN
  // map PosTable actions
  CASE io.nActionID OF
    ACTIONID_NO_ACTION:
      bDefaultActionDone := TRUE;

    ACTIONID_WAIT_ONE_SCAN:
      bDefaultActionDone := TRUE;

    ACTIONID_WAIT_STEP:
      bWaiting := TRUE;
      bDefaultActionDone := bOsrStep;
  ELSE
    bDefaultActionDone := io.nActionID <= RESERVED_ACTIONID_UBOUND;
  END_CASE;
ELSE
  bDefaultActionDone := FALSE;
END_IF;


// -----------------------------------------------------------------------------
// state monitoring
// Waiting condition is used to exclude the time where it is waiting for the Step button
// -----------------------------------------------------------------------------
io.nStateTime := MceStateMonitoring(nState := io.nSmPosTable,
                                      bFreezeTimer := bWaiting OR NOT io.bSystemReady OR (io.nSmPosTable >= 40) OR (io.nSmPosTable < 10),
                                      stateData := stStateMonitoringData);


// -----------------------------------------------------------------------------
// process move commands
// -----------------------------------------------------------------------------
// Rising edge signals of Enable
FOR i := 0 TO QA_MAX DO
  aOsrEnable[i] := aEnable[i] AND NOT aOneShots[i+6];
  aOneShots[i+6] := aEnable[i];
END_FOR;

// map parameterdata to move FB's
FOR i := 0 TO QA_MAX DO
  IF aOsrEnable[i] THEN

    McePosTableUpdateLoadIndex( allowQueuing := bAllowQueuing,
                                firstMove := bFirstMove,
                                stopAfterLastEntry := ((io.nCycleNr + 1) >= io.nNrOfCycles) AND NOT (io.nNrOfCycles = 0),
                                conditions := io.aConditions,
                                index := io.nIndex,
                                loadIndex := io.nLoadIndex,
                                QA := nQA,
                                posTable := posTable);
    aUseMoveType[i] := posTable.stEntry[io.nLoadIndex].nMoveType;

    IF aUseMoveType[i] = 1 THEN
      // -------------------------------------
      // mapping linear move FB'S
      // -------------------------------------
      fbRobotMoveLinearAbsolute[i].RobotNumber := io.nRobotNumber;
      fbRobotMoveLinearAbsolute[i].TargetType := posTable.stEntry[io.nLoadIndex].bTargetType;

      // target position
      MEMUtils.MemSet(pbyBuffer := ADR(fbRobotMoveLinearAbsolute[i].TargetPosition), byValue :=  0, dwSize :=  SIZEOF(fbRobotMoveLinearAbsolute[i].TargetPosition));
      IF posTable.stEntry[io.nLoadIndex].bTargetType THEN
        // as TCP position
        MEMUtils.MemCpy(pbyDest := ADR(fbRobotMoveLinearAbsolute[i].TargetPosition.TCPPosition.TCPPosition), pbySrc :=  ADR(posTable.stEntry[io.nLoadIndex].aPosition), dwSize :=  SIZEOF(posTable.stEntry[io.nLoadIndex].aPosition));
      ELSE
        // as axis positions
        MEMUtils.MemCpy(pbyDest := ADR(fbRobotMoveLinearAbsolute[i].TargetPosition.AxisPosition), pbySrc :=  ADR(posTable.stEntry[io.nLoadIndex].aPosition), dwSize :=  SIZEOF(posTable.stEntry[io.nLoadIndex].aPosition));
      END_IF;

      // blending between motions
      fbRobotMoveLinearAbsolute[i].BlendFactor := SINT_TO_DINT(posTable.stEntry[io.nLoadIndex].nBlendFactor);
      fbRobotMoveLinearAbsolute[i].BlendType := 0;
      fbRobotMoveLinearAbsolute[i].Speed := posTable.stEntry[io.nLoadIndex].fSpeed;
      fbRobotMoveLinearAbsolute[i].UseRotationalSpeed := posTable.stEntry[io.nLoadIndex].bUseRotationalSpeed;
      fbRobotMoveLinearAbsolute[i].SpeedUnits := posTable.stEntry[io.nLoadIndex].nSpeedUnits;
      fbRobotMoveLinearAbsolute[i].Acceleration := SINT_TO_REAL(posTable.stEntry[io.nLoadIndex].nAcc);
      fbRobotMoveLinearAbsolute[i].Deceleration := SINT_TO_REAL(posTable.stEntry[io.nLoadIndex].nDec);
    ELSE
      // -------------------------------------
      // mapping PtP move FB's
      // -------------------------------------
      fbRobotMoveAxisAbsolute[i].RobotNumber := io.nRobotNumber;
      fbRobotMoveAxisAbsolute[i].TargetType := posTable.stEntry[io.nLoadIndex].bTargetType;

      // target position
      MEMUtils.MemSet(pbyBuffer := ADR(fbRobotMoveAxisAbsolute[i].TargetPosition), byValue :=  0, dwSize :=  SIZEOF(fbRobotMoveAxisAbsolute[i].TargetPosition));
      IF posTable.stEntry[io.nLoadIndex].bTargetType THEN
        // as TCP position
        MEMUtils.MemCpy(pbyDest := ADR(fbRobotMoveAxisAbsolute[i].TargetPosition.TCPPosition.TCPPosition), pbySrc :=  ADR(posTable.stEntry[io.nLoadIndex].aPosition), dwSize :=  SIZEOF(posTable.stEntry[io.nLoadIndex].aPosition));

      ELSE
        // as axis positions
        MEMUtils.MemCpy(pbyDest := ADR(fbRobotMoveAxisAbsolute[i].TargetPosition.AxisPosition), pbySrc :=  ADR(posTable.stEntry[io.nLoadIndex].aPosition), dwSize :=  SIZEOF(posTable.stEntry[io.nLoadIndex].aPosition));
      END_IF;

      // blending between motions
      fbRobotMoveAxisAbsolute[i].BlendFactor := SINT_TO_DINT(posTable.stEntry[io.nLoadIndex].nBlendFactor);
      fbRobotMoveAxisAbsolute[i].BlendType := 0;
      fbRobotMoveAxisAbsolute[i].Speed := posTable.stEntry[io.nLoadIndex].fSpeed;
      fbRobotMoveAxisAbsolute[i].Acceleration := SINT_TO_REAL(posTable.stEntry[io.nLoadIndex].nAcc);
      fbRobotMoveAxisAbsolute[i].Deceleration := SINT_TO_REAL(posTable.stEntry[io.nLoadIndex].nDec);
    END_IF;

    // -------------------------------------
    // mapping SetUserFrame FB's
    // -------------------------------------
    fbRobotSetUserFrame[i].RobotNumber := io.nRobotNumber;
    nUserFrameNr := SINT_TO_DINT(posTable.stEntry[io.nLoadIndex].nUserFrameNumber);
    fbRobotSetUserFrame[i].UserFrameNumber := nUserFrameNr;
    fbRobotSetUserFrame[i].UserFrameData.CoordFrame := userFrames.aFrames[nUserFrameNr].aVector;

    // change user frame; if the user frame number differs from the previous stEntry a MLxRobotSetUserFrame will be triggered just before the move
    aChangeUserFrame[i] := bFirstMove OR (nUserFrameNr <> INT_TO_DINT(MLX.Robot[io.nRobotNumber].ActiveUserFrameNumber));

    // -------------------------------------
    // mapping ToolSelect FB's
    // -------------------------------------
    fbRobotSelectTool[i].RobotNumber := io.nRobotNumber;
    nToolNr := SINT_TO_DINT(posTable.stEntry[io.nLoadIndex].nToolNumber);
    fbRobotSelectTool[i].ToolNumber := nToolNr;
    fbRobotSelectTool[i].ToolData := tools.aTools[nToolNr];

    // change (select) tool; if the tool number differs from the previous stEntry a MLxRobotSelectTool will be triggered just before the move
    aChangeTool[i] := bFirstMove OR (nToolNr <> INT_TO_DINT(MLX.Robot[io.nRobotNumber].ActiveToolNumber));
    bFirstMove := FALSE;
  END_IF;
END_FOR;

// map Enable signals to move FB's
FOR i := 0 TO QA_MAX DO
  bUserFrameDone := NOT aChangeUserFrame[i] OR (fbRobotSetUserFrame[i].Sts_EN AND fbRobotSetUserFrame[i].Sts_DN AND NOT fbRobotSetUserFrame[i].Sts_ER);
  bToolDone := NOT aChangeTool[i] OR (fbRobotSelectTool[i].Sts_EN AND fbRobotSelectTool[i].Sts_DN AND NOT fbRobotSelectTool[i].Sts_ER);
  fbRobotMoveAxisAbsolute[i].Enable := aEnable[i] AND NOT (aUseMoveType[i] = 1) AND bUserFrameDone AND bToolDone;
  fbRobotMoveLinearAbsolute[i].Enable := aEnable[i] AND (aUseMoveType[i] = 1) AND bUserFrameDone AND bToolDone;
  fbRobotSetUserFrame[i].Enable := aEnable[i] AND aChangeUserFrame[i];
  fbRobotSelectTool[i].Enable := aEnable[i] AND aChangeTool[i];
END_FOR;


// -----------------------------------------------------------------------------
// FB calls (order is important!)
// -----------------------------------------------------------------------------
FOR i := 0 TO QA_MAX DO
  fbRobotSelectTool[i](MLX := MLX);
  fbRobotSetUserFrame[i](MLX := MLX);
  fbRobotMoveAxisAbsolute[i](MLX := MLX);
  fbRobotMoveLinearAbsolute[i](MLX := MLX);
END_FOR;

// -----------------------------------------------------------------------------
// map status signals from move FB's
// -----------------------------------------------------------------------------
FOR i := 0 TO QA_MAX DO
  IF aUseMoveType[i] = 1 THEN
    aMoveCmd[i].bSts_EN := fbRobotMoveLinearAbsolute[i].Sts_EN;
    aMoveCmd[i].bSts_DN := fbRobotMoveLinearAbsolute[i].Sts_DN;
    aMoveCmd[i].bSts_IP := fbRobotMoveLinearAbsolute[i].Sts_IP;
    aMoveCmd[i].bSts_AC := fbRobotMoveLinearAbsolute[i].Sts_AC;
    aMoveCmd[i].bSts_PC := fbRobotMoveLinearAbsolute[i].Sts_PC;
    aMoveCmd[i].bSts_ER := fbRobotMoveLinearAbsolute[i].Sts_ER;
    aMoveCmd[i].nPercentComplete := fbRobotMoveLinearAbsolute[i].PercentComplete;
  ELSE
    aMoveCmd[i].bSts_EN := fbRobotMoveAxisAbsolute[i].Sts_EN;
    aMoveCmd[i].bSts_DN := fbRobotMoveAxisAbsolute[i].Sts_DN;
    aMoveCmd[i].bSts_IP := fbRobotMoveAxisAbsolute[i].Sts_IP;
    aMoveCmd[i].bSts_AC := fbRobotMoveAxisAbsolute[i].Sts_AC;
    aMoveCmd[i].bSts_PC := fbRobotMoveAxisAbsolute[i].Sts_PC;
    aMoveCmd[i].bSts_ER := fbRobotMoveAxisAbsolute[i].Sts_ER;
    aMoveCmd[i].nPercentComplete := fbRobotMoveAxisAbsolute[i].PercentComplete;
  END_IF;
END_FOR;


// -----------------------------------------------------------------------------
// map nPercentComplete
// -----------------------------------------------------------------------------
CASE io.nSmPosTable OF
  10..29:
    io.nPercentComplete := aMoveCmd[nInstanceNumber[0]].nPercentComplete;
  30..50:
    io.nPercentComplete := 100;
  ELSE
    io.nPercentComplete := 0;
END_CASE;


// -----------------------------------------------------------------------------
// PosTable status
// -----------------------------------------------------------------------------
io.bWaitForStep := bWaiting;
io.bDone := (io.nSmPosTable = 50);
io.bIdle := (io.nSmPosTable < 5);
io.bBusy := (io.nSmPosTable >= 5 AND io.nSmPosTable < 50);
io.aMissingConditions := stConditionResult.aMissingConditions;
bError := MLX.JoggingMode;
FOR i := 0 TO QA_MAX DO
  bError := bError OR (aMoveCmd[i].bSts_EN AND aMoveCmd[i].bSts_ER)
        OR (fbRobotSelectTool[i].Sts_EN AND fbRobotSelectTool[i].Sts_ER)
        OR (fbRobotSetUserFrame[i].Sts_EN AND fbRobotSetUserFrame[i].Sts_ER);
END_FOR;
IF bError AND NOT (io.nSmPosTable = 99) THEN
  io.nErrorCode := 1000 + INT_TO_UDINT(io.nSmPosTable);
  io.nSmPosTable := 99;
END_IF;

fbPosTable : ARRAY[0..0] OF McePosTable;

fbPosTable[0]( io := dummy,  MLX := dummy,  posTable := dummy,  userFrames := dummy,  tools := dummy );