Servo moottorin nopeusohjaus

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