ServoDriver

S31M1.Reset = NOT_AUS AND vReset31M1(*ENABLE31M1*)
S31M1.EnableMovement = HydrP2
S31M1.DeviceName = DeviceNameS31M1
S31M1.MovPosition = MovPositionS31M1
S31M1.SetupMode = DS1EINRIC
S31M1.ManSlowestSpeed = DS1EINZEL
S31M1.ManSlowSpeed = DS1SCHLEI
S31M1.ManFastSpeed = DS1EILGAN
S31M1.ManDirection = DS1RICHTU
S31M1.SlowestSpeed = S31SlowestSpeed
S31M1.SlowSpeed = S31SlowSpeed
S31M1.FastSpeed = S31FastSpeed
S31M1.AutoPosition = S31M1AutoPosition
S31M1.mmpp = 0.00498 // mmm per pulse conf
S31M1.RefPointStart = 815 ;[PULSE]
S31M1.RefPointEnd = 845 ;[PULSE]
S31M1.RefDetect = M011_UberlDSr // Input related => %IX."X20DI9371e".DigitalInput07
S31M1.Zero = VWZ1Null
S31M1.OvershootREQ = S31M1OvershootREQ

S31M1 FUB ServoDriverS31()

MovEnableS31M1 = S31M1.MovEnable
MovSpeedS31M1 = S31M1.MovSpeed
RPSM1 = S31M1.RefPoint
VWZ1AUS1 = S31M1.Pos_GT_SP
VWZ1AUS2 = S31M1.Pos_LT_SP
VWZ1AUS3 = S31M1.Pos_EQ_SP
S31M1Position = S31M1.Position

(********************************************************************
 * COPYRIGHT --
 ********************************************************************
 * Program: Drivers
 * File: ServoDriverS31.ab
 * Author: Deniss
 * Created: September 10, 2016
 ********************************************************************
 * Implementation of program Drivers
 ********************************************************************)

(* TODO: Add your comment here *)
FUNCTION_BLOCK ServoDriverS31
    // Mode 1 - position, Mode 2 - speed, Mode 128 - reset
    IF (NOT Reset) THEN
        Mode = 128
        ManSlowestSpeed = FALSE
        ManSlowSpeed = FALSE
        ManFastSpeed = FALSE
        SetupMode = FALSE
        HomePosition = MovPosition
    ELSE
        Mode = 2
    ENDIF

    // Initialize HomePosition on Zero signal positive edge
    // Used in Position calculation
    IF EDGEPOS(Zero) THEN
        HomePosition = MovPosition
    ENDIF

    // Initialize RefStartPosition on RefDetect signal positive edge
    // Used in Reference Point calculation
    IF EDGEPOS(RefDetect) THEN
        RefStartPosition = MovPosition
    ENDIF


    // Write Mode to module
    AsIOAccWrite_FB.enable = TRUE
    AsIOAccWrite_FB.pDeviceName = ADR(DeviceName)
    AsIOAccWrite_FB.pChannelName = ADR(ChannelNameMovementControl)
    AsIOAccWrite_FB.value = Mode
    AsIOAccWrite_FB FUB AsIOAccWrite()

    // Calculate current position
    Position = REAL(MovPosition - HomePosition) * mmpp
    // Generate position outputs for program
    // Pos_EQ_SP - servo within margin of 0.1mm from SP.
    // Pos_LT_SP - servo position lower than SP
    // Pos_GT_SP - servo position higher than SP
    IF OvershootREQ THEN
        AutoPosition = AutoPosition - 0.1
    ENDIF

    Pos_EQ_SP = ABS( Position - AutoPosition ) <= 0.1
    Pos_LT_SP = MovPosition < HomePosition + TRUNC(AutoPosition / mmpp)
    Pos_GT_SP = MovPosition > HomePosition + TRUNC(AutoPosition / mmpp)


    // Reference point detection
    // MovPosition>HomePosition + Xmm, MovPosition<HomePosition + Xmm + Ymm
    RefPoint = (MovPosition > RefStartPosition + TRUNC(RefPointStart) ) AND (MovPosition < RefStartPosition + TRUNC(RefPointEnd))

    SpeedHZ = 0.0
    MovSpeed = 0

    IF SetupMode THEN
        IF ManFastSpeed AND EnableMovement THEN
            SpeedHZ = FastSpeed
        ENDIF
        IF ManSlowSpeed AND EnableMovement THEN
            SpeedHZ = SlowSpeed
        ENDIF
        IF ManSlowestSpeed AND EnableMovement THEN
            SpeedHZ = SlowestSpeed
        ENDIF
        IF NOT ManDirection THEN
            SpeedHZ = -1 * SpeedHZ
        ENDIF
        SpeedHZ = LIMIT(-100000.0, SpeedHZ, 100000.0)
        MovSpeed = TRUNC(SpeedHZ * 16777216 / 1000 * 2)
    ENDIF

    MovEnable = (Mode = 2)

END_FUNCTION_BLOCK