Join us today!

Notifications

Clear all

Inquiry: Actual Position Leading Target Position in TwinCAT MoveAbsolute

How-to & Troubleshooting

Last Post by dxt123 2 hours ago

1 Posts

1 Users

0 Reactions

10 Views

RSS

Posts: 1

dxt123

Topic starter

Oct 14, 2026 10:02 pm

(@dxt123)

New Member

Joined: 3 hours ago

[#1559]

I am writing to inquire about an unusual issue we have encountered while controlling a servo motor using the TwinCAT system.

Currently, we are operating the motor by inputting the target position and velocity into the MoveAbsolute function block. While monitoring the servo motor's actual position (GVL.Axis1.NcToPlc.ModuloActPos) using the Scope function, we noticed a strange behavior: during the initial phase of the movement, the actual position lags behind the target position. However, after a certain point, the actual position starts to lead the target position by approximately 1 degree.

Unlike a standard following error (lagging), it is difficult for us to pinpoint why the motor would move ahead of the target position. Therefore, we are reaching out for your assistance.

To help with your investigation, I have attached the relevant test files. In the attached test_profile.csv file, 'F' represents the velocity and 'U' represents the target position.

We would greatly appreciate your insights into the potential causes of this issue and any recommendations on additional parameters or settings we should inspect.

Additionally, I have provided the PLC code below.

Thank you for your support.

PROGRAM MAIN

VAR

Robot1: FB_FANUC_Basic;

bReset: BOOL;

RobotEnable, RobotFault: BOOL;

// Buffer

send_single_buffer_v2 : ARRAY[0..6] OF REAL;

goto_buffer : ARRAY[0..6] OF REAL; // Buffer to move robot for manual

send_buffer1_1_v2, send_buffer1_2_v2, send_buffer1_3_v2,

send_buffer2_1_v2, send_buffer2_2_v2, send_buffer2_3_v2: ARRAY[0..146] OF REAL;

// Function Block

fb_custom_service : FB_CUSTOM_SERVICE; // Send a Sequence Data to Robot

fbMove1, fbMove2, fbHome : MC_MoveModulo; // Turn Table Motor Move, Only Turn Table Motor Move, Turn Table Motor Homing

fbMove : MC_MoveAbsolute;

fbMove_rev1, fbMove_rev2, fbMove_rot1, fbMove_rot2 : MC_MoveVelocity; // Revolution & Rotation Motor Move

fbReset : MC_Reset; // Motor Reset

fbPower, fbPower_rev, fbPower_rot : MC_Power; // Motor Power On

mc_readactpos : MC_ReadActualPosition; // Read Position of Turn Table Motor

mc_readactvel_rev, mc_readactvel_rot : MC_ReadActualVelocity; // Read Velocity of Revolution & Rotation Motor

mc_stop, mc_stop_rev, mc_stop_rot : MC_Stop; // Motor Stop

mc_setpos: MC_SetPosition;

bSetDone: BOOL;

fbTimer_SetPos: TON;

bTimer: BOOL;

REdge_setdone: R_TRIG;

// Variables

// Bool

bExecute : BOOL; // fb_custom_service.Execute

motor_trig1, motor_trig2 : BOOL; // Signal for cross-action of Motors

bStopDone, bStopDone_rev, bStopDone_rot : BOOL; // Stopped Rising Edge Signal

bHomeDone : BOOL; // Hoiming Done Signal

// LREAL

nPos, nVel : LREAL; // Input Data of Turn Table Motor

fServoCurrPos, fServoCurrVel_rev, fServoCurrVel_rot : LREAL; // Output Data of the Motors

// INT

nState : INT:= 0;

nPointIdx : INT:= 1;

trig_status : INT:= 0;

// fb_custom_service variables

nServiceCode : BYTE;

nClass, nInstance, nAttribute : WORD;

nNumberofData : DINT;

nBufferID : INT;

pCurrentAddr : PVOID;

nCurrentSize : WORD;

nErrId : UDINT;

error : STRING;

// Rising Edge Trigger

REdge_first : R_TRIG;

REdge_StopDone : R_TRIG;

REdge_StopDone_rev : R_TRIG;

REdge_StopDone_rot : R_TRIG;

REdge_Restart : R_TRIG;

REdge_HomeDone : R_TRIG;

REdge_DO48 : R_TRIG;

bWaitCheckPoint: BOOL;

nNextState: INT;

bRobotAck: BOOL;

bInit : BOOL:= TRUE;

bFirstStart : BOOL:= TRUE;

bNextStepReq : BOOL;

nCntIdx : INT;

fbScanTimer : TON;

fbSetOverride : MC_SetOverride;

fRobotFirstPos: LREAL;

fRobotStartPos, fRobotTargetPos, fRobotActualPos,

fRobotTotalDist, fRobotRemainRatio,

fMotorStartPos, fMotorTargetPos, fMotorActualPos,

fMotorTotalDist, fMotorVirtualTarget : LREAL;

fSampledTargetPos, fSampledActualPos : LREAL;

fOutput : LREAL;

fCalculatedOverride : LREAL:= 0.0;

nPos_X: UDINT:= 0;

nPos_Y: UDINT:= 0;

fBackupValue: LREAL;

fRelativePos: LREAL;

REdge_bStart: R_TRIG;

fScaled_Kp: LREAL;

fDrivePosLoopGain: LREAL:= 32.0;

fPositionError: LREAL;

bSequenceMode: BOOL:= FALSE;

nXorY: INT;

nRobotIdx: UDINT:=0;

nRobotIdx_int: INT;

frobot: LREAL;

nTrackingIdx: INT:= 1;

nTotalProcessCnt: DINT:= 1;

END_VAR

Robot1.Update();

RobotEnable:= Robot1.qEnable;

RobotFault:= Robot1.qFault;

CASE nBufferID OF

pCurrentAddr:= ADR(send_single_buffer_v2);

nCurrentSize:= SIZEOF(send_single_buffer_v2);

pCurrentAddr:= ADR(goto_buffer);

nCurrentSize:= SIZEOF(goto_buffer);

11:

pCurrentAddr:= ADR(send_buffer1_1_v2);

nCurrentSize:= SIZEOF(send_buffer1_1_v2);

12:

pCurrentAddr:= ADR(send_buffer1_2_v2);

nCurrentSize:= SIZEOF(send_buffer1_2_v2);

13:

pCurrentAddr:= ADR(send_buffer1_3_v2);

nCurrentSize:= SIZEOF(send_buffer1_3_v2);

21:

pCurrentAddr:= ADR(send_buffer2_1_v2);

nCurrentSize:= SIZEOF(send_buffer2_1_v2);

22:

pCurrentAddr:= ADR(send_buffer2_2_v2);

nCurrentSize:= SIZEOF(send_buffer2_2_v2);

23:

pCurrentAddr:= ADR(send_buffer2_3_v2);

nCurrentSize:= SIZEOF(send_buffer2_3_v2);

99:

pCurrentAddr:= ADR(nNumberofData);

nCurrentSize:= SIZEOF(nNumberofData);

END_CASE

fb_custom_service(

sNetId:= '192.168.0.196.3.1',

sIPv4Addr:= '100.100.100.1',

bExecute:= bExecute,

nServiceCode:= nServiceCode,

nClass:= nClass,

nInstance:= nInstance,

nAttribute:= nAttribute,

pSrc:= pCurrentAddr,

nSrcDataLen:= nCurrentSize,

);

error:= F_GET_ETHERNETIP_ERROR_TEXT(nErrorId:= nErrId);

fbPower(Axis:= GVL.Axis1, Enable:= TRUE, Enable_Positive:= TRUE, Enable_Negative:= TRUE, Override:= 100.0);

fbPower_rev(Axis:= GVL.Axis2, Enable:= TRUE, Enable_Positive:= TRUE, Enable_Negative:= TRUE, Override:= 100.0);

fbPower_rot(Axis:= GVL.Axis3, Enable:= TRUE, Enable_Positive:= TRUE, Enable_Negative:= TRUE, Override:= 100.0);

fbReset(Axis:= GVL.Axis1, Execute:= GVL.bReset);

IF GVL.bReset THEN

nState:= 0;

GVL.bStart:= FALSE;

GVL.bReqUpdateLower:= FALSE;

GVL.bReqUpdateUpper:= FALSE;

END_IF

CASE nState OF

IF GVL.bStart THEN

nPointIdx:= 1;

nState:= 10;

END_IF

10:

fbMove1.Position:= GVL.aPathData[nPointIdx].fPosition;

fbMove1.Velocity:= GVL.aPathData[nPointIdx].fVelocity;

fbMove1.BufferMode:= MC_BufferMode.MC_Aborting;

fbMove1.Direction:= MC_Positive_Direction;

fbMove1.Execute:= TRUE;

fbMove_rev1.Velocity:= GVL.aPathData[nPointIdx].fVelocity2;

fbMove_rev1.BufferMode:= MC_BufferMode.MC_Aborting;

fbMove_rev1.Execute:= TRUE;

fbMove_rot1.Velocity:= GVL.aPathData[nPointIdx].fVelocity3;

fbMove_rot1.BufferMode:= MC_BufferMode.MC_Aborting;

fbMove_rot1.Execute:= TRUE;

IF fbMove1.Active AND fbMove_rev1.Active AND fbMove_rot1.Active THEN

fbMove1.Execute:= FALSE;

fbMove_rev1.Execute:= FALSE;

fbMove_rot1.Execute:= FALSE;

nPointIdx:= nPointIdx + 1;

motor_trig1:= TRUE;

nState:= 20;

END_IF

20:

fbMove2.Position:= GVL.aPathData[nPointIdx].fPosition;

fbMove2.Velocity:= GVL.aPathData[nPointIdx].fVelocity;

fbMove2.BufferMode:= MC_BufferMode.MC_BlendingPrevious;

fbMove2.Direction:= MC_Positive_Direction;

fbMove_rev2.Velocity:= GVL.aPathData[nPointIdx].fVelocity2;

fbMove_rev2.BufferMode:= MC_BufferMode.MC_BlendingPrevious;

fbMove_rot2.Velocity:= GVL.aPathData[nPointIdx].fVelocity3;

fbMove_rot2.BufferMode:= MC_BufferMode.MC_BlendingPrevious;

IF motor_trig1 THEN

IF GVL.aPathData[nPointIdx].fVelocity > 0.0 THEN

fbMove2.Execute:= TRUE;

fbMove_rev2.Execute:= TRUE;

fbMove_rot2.Execute:= TRUE;

ELSE

motor_trig1:= FALSE;

nState:= 99;

END_IF

IF fbMove2.Active AND fbMove_rev2.Active AND fbMove_rot2.Active THEN

fbMove2.Execute:= FALSE;

fbMove_rev2.Execute:= FALSE;

fbMove_rot2.Execute:= FALSE;

motor_trig1:= FALSE;

nPointIdx:= nPointIdx + 1;

nTotalProcessCnt:= nTotalProcessCnt + 1;

IF (nTotalProcessCnt > 1) AND (((nTotalProcessCnt - 1) MOD 63) = 0) THEN

bWaitCheckPoint:= TRUE;

END_IF

IF nPointIdx = 501 THEN

GVL.bReqUpdateLower:= TRUE;

ELSIF nPointIdx > GVL.nMaxPoints THEN

nPointIdx:= 1;

IF NOT GVL.bAllDataFinished THEN

GVL.bReqUpdateUpper:= TRUE;

END_IF

IF nState <> 99 THEN

IF bWaitCheckPoint THEN

nNextState:= 30;

nState:= 40;

ELSE

motor_trig2:= TRUE;

nState:= 30;

END_IF

ELSIF fbMove2.Error THEN

nState:= 900;

END_IF

30:

fbMove1.Position:= GVL.aPathData[nPointIdx].fPosition;

fbMove1.Velocity:= GVL.aPathData[nPointIdx].fVelocity;

fbMove1.BufferMode:= MC_BufferMode.MC_BlendingPrevious;

fbMove1.Direction:= MC_Positive_Direction;

fbMove_rev1.Velocity:= GVL.aPathData[nPointIdx].fVelocity2;

fbMove_rev1.BufferMode:= MC_BufferMode.MC_BlendingPrevious;

fbMove_rot1.Velocity:= GVL.aPathData[nPointIdx].fVelocity3;

fbMove_rot1.BufferMode:= MC_BufferMode.MC_BlendingPrevious;

IF motor_trig2 THEN

IF GVL.aPathData[nPointIdx].fVelocity > 0.0 THEN

fbMove1.Execute:= TRUE;

fbMove_rev1.Execute:= TRUE;

fbMove_rot1.Execute:= TRUE;

ELSE

motor_trig2:= FALSE;

nState:= 99;

END_IF

IF fbMove1.Active AND fbMove_rev1.Active AND fbMove_rot1.Active THEN

fbMove1.Execute:= FALSE;

fbMove_rev1.Execute:= FALSE;

fbMove_rot1.Execute:= FALSE;

motor_trig2:= FALSE;

nPointIdx:= nPointIdx + 1;

nTotalProcessCnt:= nTotalProcessCnt + 1;

IF (nTotalProcessCnt > 1) AND (((nTotalProcessCnt - 1) MOD 63) = 0) THEN

bWaitCheckPoint:= TRUE;

END_IF

IF nPointIdx = 501 THEN

GVL.bReqUpdateLower:= TRUE;

ELSIF nPointIdx > GVL.nMaxPoints THEN

nPointIdx:= 1;

IF NOT GVL.bAllDataFinished THEN

GVL.bReqUpdateUpper:= TRUE;

END_IF

IF nState <> 99 THEN

IF bWaitCheckPoint THEN

nNextState:= 20;

nState:= 40;

ELSE

motor_trig1:= TRUE;

nState:= 20;

END_IF

ELSIF fbMove1.Error THEN

nState:= 900;

END_IF

40:

IF Robot1._UO1.DO237 THEN

Robot1._UI1.DI44:= TRUE;

bRobotAck:= TRUE;

END_IF

IF bRobotAck AND NOT Robot1._UO1.DO237 THEN

Robot1._UI1.DI44:= FALSE;

bRobotAck:= FALSE;

bWaitCheckPoint:= FALSE;

IF nNextState = 20 THEN

motor_trig1 := TRUE;

ELSIF nNextState = 30 THEN

motor_trig2 := TRUE;

END_IF

nState := nNextState;

END_IF

99:

IF NOT fbMove1.Busy AND NOT fbMove2.Busy THEN

IF Robot1._UO1.DO237 THEN

Robot1._UI1.DI44:= TRUE;

ELSE

Robot1._UI1.DI44:= FALSE;

nState:= 0;

nPointIdx:= 1;

nTrackingIdx:= 1;

motor_trig1:= FALSE;

motor_trig2:= FALSE;

bWaitCheckPoint:= FALSE;

bRobotAck:= FALSE;

nNextState:= 0;

fbMove1.Execute:= FALSE;

fbMove2.Execute:= FALSE;

fbMove_rev1.Execute:= FALSE;

fbMove_rev2.Execute:= FALSE;

fbMove_rot1.Execute:= FALSE;

fbMove_rot2.Execute:= FALSE;

GVL.bStart:= FALSE;

IF NOT GVL.bStop_rev THEN

GVL.bStop_rev:= TRUE;

END_IF

IF NOT GVL.bStop_rot THEN

GVL.bStop_rot:= TRUE;

END_IF

bSequenceMode:= FALSE;

END_IF

900:

IF NOT fbMove1.Busy AND NOT fbMove2.Busy THEN

GVL.bStart:= FALSE;

nState:= 0;

nPointIdx:= 1;

nCntIdx:= 0;

bSequenceMode:= FALSE;

END_IF

END_CASE

fbMove1(Axis:= GVL.Axis1);

fbMove2(Axis:= GVL.Axis1);

fbMove_rev1(Axis:= GVL.Axis2);

fbMove_rev2(Axis:= GVL.Axis2);

fbMove_rot1(Axis:= GVL.Axis3);

fbMove_rot2(Axis:= GVL.Axis3);

IF GVL.bReqUpdateLower AND GVL.bUpdateDone THEN

GVL.bReqUpdateLower:= FALSE;

END_IF

IF GVL.bReqUpdateUpper AND GVL.bUpdateDone THEN

GVL.bReqUpdateUpper:= FALSE;

END_IF

REdge_first(CLK:= MAIN.Robot1._UO1.DO45);

IF REdge_first.Q THEN

GVL.bStart:= TRUE;

END_IF

fbMove(

Axis:= GVL.Axis1,

Execute:= GVL.bStart2,

Position:= nPos,

Velocity:= nVel

);

fbHome(

Axis:= GVL.Axis1,

Execute:= GVL.bHome,

Position:= 0.0,

Velocity:= 10,

Direction:= MC_Direction.MC_Shortest_Way,

Done=> bHomeDone,

);

mc_readactpos(

Axis:= GVL.Axis1,

Enable:= TRUE,

Position=> fServoCurrPos

);

mc_readactvel_rev(

Axis:= GVL.Axis2,

Enable:= TRUE,

ActualVelocity=> fServoCurrVel_rev

);

mc_readactvel_rot(

Axis:= GVL.Axis3,

Enable:= TRUE,

ActualVelocity=> fServoCurrVel_rot

);

mc_stop(

Axis:= GVL.Axis1,

Execute:= GVL.bStop,

Done=> bStopDone,

);

mc_stop_rev(

Axis:= GVL.Axis2,

Execute:= GVL.bStop_rev,

Done=> bStopDone_rev,

);

mc_stop_rot(

Axis:= GVL.Axis3,

Execute:= GVL.bStop_rot,

Done=> bStopDone_rot,

);

mc_setpos(

Axis:= GVL.Axis1,

Position:= 0.0,

Done=> bSetDone

);

REdge_HomeDone(CLK:= bHomeDone);

IF REdge_HomeDone.Q THEN

GVL.bHome:= FALSE;

bTimer:= TRUE;

END_IF

fbTimer_SetPos(IN:= bTimer, PT:= T#1S);

IF fbTimer_SetPos.Q THEN

mc_setpos.Execute:= TRUE;

END_IF

REdge_setdone(CLK:= bSetDone);

IF REdge_setdone.Q THEN

mc_setpos.Execute:= FALSE;

bTimer:= FALSE;

END_IF

REdge_StopDone(CLK:= bStopDone);

IF REdge_StopDone.Q THEN

GVL.bStart := FALSE;

GVL.bStart2 := FALSE;

IF motor_trig1 THEN

trig_status:= 1;

motor_trig1:= FALSE;

fbMove2.Execute:= FALSE;

fbMove_rev2.Execute:= FALSE;

fbMove_rot2.Execute:= FALSE;

ELSIF motor_trig2 THEN

trig_status:= 2;

motor_trig2:= FALSE;

fbMove1.Execute:= FALSE;

fbMove_rev1.Execute:= FALSE;

fbMove_rot1.Execute:= FALSE;

END_IF

GVL.bStop:= FALSE;

END_IF

REdge_StopDone_rev(CLK:= bStopDone_rev);

IF REdge_StopDone_rev.Q THEN

GVL.bStop_rev:= FALSE;

END_IF

REdge_StopDone_rot(CLK:= bStopDone_rot);

IF REdge_StopDone_rot.Q THEN

GVL.bStop_rot:= FALSE;

END_IF

REdge_Restart(CLK:= GVL.bRestart);

IF REdge_Restart.Q THEN

GVL.bStart:= TRUE;

IF trig_status = 1 THEN

motor_trig1:= TRUE;

ELSIF trig_status = 2 THEN

motor_trig2:= TRUE;

END_IF

GVL.bRestart:= FALSE;

END_IF

nPos_X.0:= Robot1._UO1.Xl0;

nPos_X.1:= Robot1._UO1.Xl1;

nPos_X.2:= Robot1._UO1.Xl2;

nPos_X.3:= Robot1._UO1.Xl3;

nPos_X.4:= Robot1._UO1.Xl4;

nPos_X.5:= Robot1._UO1.Xl5;

nPos_X.6:= Robot1._UO1.Xl6;

nPos_X.7:= Robot1._UO1.Xl7;

nPos_X.8:= Robot1._UO1.Xl8;

nPos_X.9:= Robot1._UO1.Xl9;

nPos_X.10:= Robot1._UO1.Xl10;

nPos_X.11:= Robot1._UO1.Xl11;

nPos_X.12:= Robot1._UO1.Xl12;

nPos_X.13:= Robot1._UO1.Xl13;

nPos_X.14:= Robot1._UO1.Xl14;

nPos_X.15:= Robot1._UO1.Xl15;

nPos_X.16:= Robot1._UO1.Xh0;

nPos_X.17:= Robot1._UO1.Xh1;

nPos_X.18:= Robot1._UO1.Xh2;

nPos_X.19:= Robot1._UO1.Xh3;

nPos_X.20:= Robot1._UO1.Xh4;

nPos_X.21:= Robot1._UO1.Xh5;

nPos_X.22:= Robot1._UO1.Xh6;

nPos_X.23:= Robot1._UO1.Xh7;

nPos_Y.0:= Robot1._UO1.Yl0;

nPos_Y.1:= Robot1._UO1.Yl1;

nPos_Y.2:= Robot1._UO1.Yl2;

nPos_Y.3:= Robot1._UO1.Yl3;

nPos_Y.4:= Robot1._UO1.Yl4;

nPos_Y.5:= Robot1._UO1.Yl5;

nPos_Y.6:= Robot1._UO1.Yl6;

nPos_Y.7:= Robot1._UO1.Yl7;

nPos_Y.8:= Robot1._UO1.Yl8;

nPos_Y.9:= Robot1._UO1.Yl9;

nPos_Y.10:= Robot1._UO1.Yl10;

nPos_Y.11:= Robot1._UO1.Yl11;

nPos_Y.12:= Robot1._UO1.Yl12;

nPos_Y.13:= Robot1._UO1.Yl13;

nPos_Y.14:= Robot1._UO1.Yl14;

nPos_Y.15:= Robot1._UO1.Yl15;

nPos_Y.16:= Robot1._UO1.Yh0;

nPos_Y.17:= Robot1._UO1.Yh1;

nPos_Y.18:= Robot1._UO1.Yh2;

nPos_Y.19:= Robot1._UO1.Yh3;

nPos_Y.20:= Robot1._UO1.Yh4;

nPos_Y.21:= Robot1._UO1.Yh5;

nPos_Y.22:= Robot1._UO1.Yh6;

nPos_Y.23:= Robot1._UO1.Yh7;

IF nXorY = 0 THEN

fRobotActualPos:= UDINT_TO_LREAL(nPos_X) / 1000.0;

IF Robot1._UO1.X_Check THEN

fRobotActualPos:= fRobotActualPos * (-1);

END_IF

ELSIF nXorY = 1 THEN

fRobotActualPos:= UDINT_TO_LREAL(nPos_Y) / 1000.0;

IF Robot1._UO1.Y_Check THEN

fRobotActualPos:= fRobotActualPos * (-1);

END_IF

REdge_bStart(CLK:= Robot1._UI1.UI09_RSR1);

IF REdge_bStart.Q THEN

fBackupValue:= fRobotActualPos;

bSequenceMode:= TRUE;

(*fRobotTargetPos := fRobotFirstPos;

fRobotStartPos := fRobotFirstPos;

fMotorTargetPos := 0.0;

fMotorStartPos := 0.0;*)

nTotalProcessCnt:= 1;

nTrackingIdx:= 1;

fRobotTargetPos:= GVL.aPathData[1].fRobot_Pos;

fRobotStartPos:= fRobotFirstPos;

fMotorTargetPos:= GVL.aPathData[1].fPosition;

fMotorStartPos:= 0.0;

fRobotTotalDist := 0.0;

fRobotRemainRatio := 0.0;

fSampledTargetPos := GVL.Axis1.NcToPlc.ModuloActPos;

fPositionError := 0.0;

fOutput := 0.0;

fCalculatedOverride := 0.8;

END_IF

fRelativePos:= fRobotActualPos - fBackupValue;

IF bSequenceMode THEN

IF ABS(fRelativePos) >= ABS(fRobotTargetPos) THEN

nTrackingIdx:= nTrackingIdx + 1;

IF nTrackingIdx > GVL.nMaxPoints THEN

nTrackingIdx:= 1;

fRobotTargetPos:= GVL.aPathData[nTrackingIdx].fRobot_Pos;

fRobotStartPos:= GVL.aPathData[GVL.nMaxPoints].fRobot_Pos;

fMotorTargetPos:= GVL.aPathData[nTrackingIdx].fPosition;

fMotorStartPos:= GVL.aPathData[GVL.nMaxPoints].fPosition;

ELSE

fRobotTargetPos:= GVL.aPathData[nTrackingIdx].fRobot_Pos;

fRobotStartPos:= GVL.aPathData[nTrackingIdx - 1].fRobot_Pos;

fMotorTargetPos:= GVL.aPathData[nTrackingIdx].fPosition;

fMotorStartPos:= GVL.aPathData[nTrackingIdx - 1].fPosition;

END_IF

fbScanTimer(IN:= NOT fbScanTimer.Q, PT:= T#10MS);

IF fbScanTimer.Q THEN

fRobotTotalDist:= fRobotTargetPos - fRobotStartPos;

IF fRobotTotalDist <> 0 THEN

fRobotRemainRatio:= (fRobotTargetPos - fRelativePos) / fRobotTotalDist;

ELSE

fRobotRemainRatio:= 0.0;

END_IF

IF fMotorTargetPos = 0 THEN

fMotorTargetPos:= 360;

END_IF

fMotorTotalDist:= fMotorTargetPos - fMotorStartPos;

fMotorVirtualTarget:= fMotorTargetPos - (fMotorTotalDist * fRobotRemainRatio);

fSampledTargetPos:= fMotorVirtualTarget;

fSampledActualPos:= GVL.Axis1.NcToPlc.ModuloActPos;

END_IF

fScaled_Kp:= fDrivePosLoopGain / 500.0;

fPositionError:= fSampledTargetPos - fSampledActualPos;

fOutput:= fPositionError * fScaled_Kp;

fCalculatedOverride:= LIMIT(0.5, (0.8 + fOutput), 1.0);

nRobotIdx.0:= Robot1._UO1.DO221;

nRobotIdx.1:= Robot1._UO1.DO222;

nRobotIdx.2:= Robot1._UO1.DO223;

nRobotIdx.3:= Robot1._UO1.DO224;

nRobotIdx.4:= Robot1._UO1.DO225;

nRobotIdx.5:= Robot1._UO1.DO226;

nRobotIdx.6:= Robot1._UO1.DO227;

nRobotIdx.7:= Robot1._UO1.DO228;

nRobotIdx.8:= Robot1._UO1.DO229;

nRobotIdx.9:= Robot1._UO1.DO230;

nRobotIdx.10:= Robot1._UO1.DO231;

nRobotIdx.11:= Robot1._UO1.DO232;

nRobotIdx.12:= Robot1._UO1.DO233;

nRobotIdx.13:= Robot1._UO1.DO234;

nRobotIdx.14:= Robot1._UO1.DO235;

nRobotIdx.15:= Robot1._UO1.DO236;

nRobotIdx_int:= UDINT_TO_INT(nRobotIdx);

IF nRobotIdx_int >= 1 AND nRobotIdx_int <= GVL.nMaxPoints THEN

frobot := GVL.aPathData[nRobotIdx_int].fRobot_Pos;

ELSE

frobot := 0.0;

END_IF

Forum Jump:

Previous Topic

Currently viewing this topic 1 guest.

17 Forums
306 Topics
1,004 Posts
3 Online
1,248 Members

Forum Icons: Forum contains no unread posts Forum contains unread posts

Topic Icons: Not Replied Replied Active Hot Sticky Unapproved Solved Private Closed

Cookie	Duration	Description
cookielawinfo-checkbox-analytics	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Analytics".
cookielawinfo-checkbox-functional	11 months	The cookie is set by GDPR cookie consent to record the user consent for the cookies in the category "Functional".
cookielawinfo-checkbox-necessary	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookies is used to store the user consent for the cookies in the category "Necessary".
cookielawinfo-checkbox-others	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Other.
cookielawinfo-checkbox-performance	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Performance".
viewed_cookie_policy	11 months	The cookie is set by the GDPR Cookie Consent plugin and is used to store whether or not user has consented to the use of cookies. It does not store any personal data.