Page History

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

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