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=>
); |