Untitled

//Sprawdz czy safety
IF #Safety THEN
    //TRYB AUTOMATYCZNY
    IF #Mode THEN
        IF #homing THEN
            #Speed := #homingSpeedInt;
            IF #homingForward AND NOT #homingBackward THEN
                #Forward := TRUE;
            ELSE
                #Forward := FALSE;
            END_IF;
            IF #homingBackward AND NOT #homingForward THEN
                #Backward := TRUE;
            ELSE
                #Backward := FALSE;
            END_IF;
        ELSE
            IF #driveRelease THEN
                // RUCH W DODATNIE WARTOŚCI
                IF #actualPosition < #destination THEN
                    #forwardDifference := #destination - #slowGoal;
                    IF #actualPosition < #forwardDifference THEN
                        #Speed := #autoSpeedInt;
                        #Forward := TRUE;
                    END_IF;
                    IF ((#destination - #actualPosition) >= #errorMargin) AND ((#destination - #actualPosition) <= #slowGoal) THEN
                        #Speed := #slowGoalSpeedInt;
                        #Forward := TRUE;
                    ELSIF ((#destination - #actualPosition) <= #errorMargin) THEN
                        #Forward := FALSE;
                    END_IF;
                ELSE
                    #Forward := FALSE;
                END_IF;
                // RUCH W UJEMNE WARTOŚCI
                IF #actualPosition > #destination THEN
                    #backwardDifference := #destination + #slowGoal;
                    IF #actualPosition > #backwardDifference THEN
                        #Speed := #autoSpeedInt;
                        #Backward := TRUE;
                    END_IF;
                    IF ((#actualPosition - #destination) >= #errorMargin) AND ((#actualPosition - #destination) <= #slowGoal) THEN
                        #Speed := #slowGoalSpeedInt;
                        #Backward := TRUE;
                    ELSIF ((#actualPosition - #destination) <= #errorMargin) THEN
                        #Backward := FALSE;
                    END_IF;
                ELSE
                    #Backward := FALSE;
                END_IF;
            ELSE
                #Forward := FALSE;
                #Backward := FALSE;
            END_IF;
        END_IF;
    ELSE // TRYB RECZNY
        #Speed := #manualSpeedInt;
        IF #manualForward AND NOT #manualBackward THEN
            #Forward := TRUE;
        ELSE
            #Forward := FALSE;
        END_IF;
        IF #manualBackward AND NOT #manualForward THEN
            #Backward := TRUE;
        ELSE
            #Backward := FALSE;
        END_IF;
    END_IF;
ELSE
    #Forward := FALSE;
    #Backward := FALSE;
END_IF;

// predkosci
// 
IF #autoSpeed > 100 THEN
    #autoSpeed := 100;
ELSIF #autoSpeed < 0 THEN
    #autoSpeed := 0;
END_IF;

IF #manualSpeed > 100 THEN
    #manualSpeed := 100;
ELSIF #manualSpeed < 0 THEN
    #manualSpeed := 0;
END_IF;

IF #slowGoalSpeed > 100 THEN
    #slowGoalSpeed := 100;
ELSIF #slowGoalSpeed < 0 THEN
    #slowGoalSpeed := 0;
END_IF;

IF #homingSpeed > 100 THEN
    #homingSpeed := 100;
ELSIF #homingSpeed < 0 THEN
    #homingSpeed := 0;
END_IF;


#autoSpeedInt := REAL_TO_DINT((#autoSpeed / 100)*#inverterMaxFrequency*100);
#manualSpeedInt := REAL_TO_DINT((#manualSpeed / 100)*#inverterMaxFrequency*100);
#slowGoalSpeedInt := REAL_TO_DINT((#slowGoalSpeed / 100)*#inverterMaxFrequency*100);
#homingSpeedInt := REAL_TO_DINT((#homingSpeed / 100)*#inverterMaxFrequency*100);