(*----------------------------------------------------------------------------*)
IF  NOT EmergencyStop    THEN
    VisionXYAlignmentBusy       := FALSE;
    VisionXYAlignmentComplete := FALSE;
    AxisRun1                   := FALSE;
    AxisRun2                   := FALSE;
    AxisRun3                   := FALSE;
END_IF;

(*----------------------------------------------------------------------------*)
IF     SaveBasePoint THEN
    InspectBasePos[0] := InspectResultPos[0];
    InspectBasePos[1] := InspectResultPos[1];
    InspectBasePos[2] := InspectResultPos[2];
    InspectBasePos[3] := InspectResultPos[3];
END_IF;

(*----------------------------------------------------------------------------*)
IF     VisionXYAlignmentStart AND NOT VisionXYAlignmentBusy    THEN
    VisionXYAlignmentBusy := TRUE;
    VisionXYAlignmentComplete := FALSE;
    VisionXYAlignmentStep := 0;
    RETURN;
END_IF;

(*----------------------------------------------------------------------------*)
IF     VisionXYAlignmentBusy    THEN
    IF     VisionXYAlignmentStep = 0    THEN
        TempFloatValue1 := InspectResultPos[0] - InspectBasePos[0];
        TempFloatValue2 := InspectResultPos[2] - InspectBasePos[2];
        TempFloatValue3 := ((TempFloatValue1 + TempFloatValue2) / 2.0) * 3.7;
        TempRunPos1 := CurrentAxisPos1 - REAL_TO_DINT(TempFloatValue3);
        
        TempFloatValue1 := InspectResultPos[1] - InspectBasePos[1];
        TempFloatValue2 := InspectResultPos[3] - InspectBasePos[3];
        TempFloatValue3 := ((TempFloatValue1 + TempFloatValue2) / 2.0) * 3.7;
        TempIntegerValue3 := ABS(REAL_TO_DINT(TempFloatValue3));
        
        IF TempFloatValue3 >= 0.0    THEN
            TempRunPos2 := CurrentAxisPos2 - TempIntegerValue3;
            TempRunPos3 := CurrentAxisPos3 + TempIntegerValue3;
        ELSE
            TempRunPos2 := CurrentAxisPos2 + TempIntegerValue3;
            TempRunPos3 := CurrentAxisPos3 - TempIntegerValue3;
        END_IF;
        
        AxisRunPos1 := TempRunPos1;
        AxisRun1 := TRUE;        
        AxisRunPos2 := TempRunPos2;
        AxisRun2 := TRUE;
        AxisRunPos3 := TempRunPos3;
        AxisRun3 := TRUE;

        VisionXYAlignmentStep := 1;
        RETURN;
    END_IF;

    IF     VisionXYAlignmentStep = 1    THEN
        IF    CurrentAxisPos1 = AxisRunPos1 AND IsAxisInPos1 AND NOT IsAxisMoving1    AND
            CurrentAxisPos2 = AxisRunPos2 AND IsAxisInPos2 AND NOT IsAxisMoving2    AND
            CurrentAxisPos3 = AxisRunPos3 AND IsAxisInPos3 AND NOT IsAxisMoving3    THEN
            AxisRun1 := FALSE;
            AxisRun2 := FALSE;
            AxisRun3 := FALSE;
        
            VisionXYAlignmentBusy := FALSE;
            VisionXYAlignmentComplete := TRUE;
        END_IF;
        RETURN;
    END_IF;
END_IF;
(*----------------------------------------------------------------------------*)

這個厲害額    加油