Drive by velo function block variables
Code Block | ||||||||||
---|---|---|---|---|---|---|---|---|---|---|
| ||||||||||
FUNCTION_BLOCK DriveByVelo VAR_INPUT setSpeed: LREAL; END_VAR VAR_OUTPUT /// Remains busy while the FB is enabled bBusy: BOOL; nErrorCode: UDINT; actualSpeed: LREAL; actualPos: LREAL; SpError: BOOL; // StZero:bool; END_VAR VAR_IN_OUT /// Reference to the axis drivedAxis: AXIS_REF; END_VAR VAR _fB_AxisError: FB_AxisError; SpBusy: BOOL; stepCounter:INT; SpErrorId: UDINT; mcReadActualVelocity: MC_ReadActualVelocity; mcReadActualPosition: MC_ReadActualPosition; State: INT; Direction: INT; //oldPos: LREAL; bBusyStop: BOOL; axisStop: MC_Stop; axisVelocity: MC_MoveVelocity; flipflop: BOOL; bStop: BOOL; END_VAR |
Drve by velo code
Code Block |
---|
IF flipflop THEN
flipflop:=FALSE;
ELSE
flipflop:=TRUE;
END_IF
mcReadActualVelocity(Enable:=TRUE,Axis:=drivedAxis, Valid=>,Busy =>, Error =>,ErrorID=>,ActualVelocity=>actualSpeed);
mcReadActualPosition(Enable:=TRUE,Axis:=drivedAxis, Valid=>,Busy =>, Error =>,ErrorID=>,Position=>actualPos);
CASE State OF
0:
stepCounter:=stepCounter+1;
IF stepCounter>5 THEN
stepCounter:=6;
END_IF
IF ABS(setSpeed)>0.001 AND stepCounter>5 THEN
_fB_AxisError(Ax:= drivedAxis);
bBusy:= TRUE;
nErrorCode:= 0;
State:= 20;
// oldPos:=actualPos;
// retToState0Counter:=0;
// StZero:=FALSE;
stepCounter:=0;
END_IF
20:
stepCounter:=0;
IF setSpeed >= 0 THEN Direction:= 1;
ELSIF setSpeed < 0 THEN Direction:=3; (*1positive 2 lyhin 3 negative 4 sama suunta kuin edellinen*)
ELSE Direction:= 4;
END_IF
setSpeed:=ABS(setSpeed);
IF setSpeed>0.001 THEN
axisVelocity(
Execute:=flipflop ,
Velocity:=setSpeed ,
Acceleration:= ,
Deceleration:= ,
Jerk:= ,
Direction:=Direction ,
BufferMode:= ,
Options:= ,
Axis:=drivedAxis ,
InVelocity=> ,
Busy=>SpBusy ,
Active=> ,
CommandAborted=> ,
Error=>SpError ,
ErrorID=>SpErrorId );
ELSE
State:= 30;
stepCounter:=0;
END_IF
30:
IF stepCounter=0 THEN
bStop:=TRUE;
ELSE
bStop:=FALSE;
END_IF
stepCounter:=stepCounter+1;
IF stepCounter >20 THEN
State:= 0;
bBusy:= FALSE;
stepCounter:=0;
END_IF
END_CASE
axisStop(
Execute:=bStop ,
Deceleration:= ,
Jerk:= ,
Options:= ,
Axis:=drivedAxis ,
Done=> ,
Busy=>bBusyStop ,
Active=> ,
CommandAborted=> ,
Error=> ,
ErrorID=>
); |