[r2]: / trunk / legacy / Libraries / SM3_Drive_Servo.library.md  Maximize  Restore  History

Download this file

278 lines (246 with data), 7.3 kB

<?xml version="1.0" encoding="utf-8"?>---

FUNCTION_BLOCK AXIS_REF_SERVO EXTENDS AXIS_REF_VIRTUAL_SM3
VAR
    pAdafruitPWM: pointer;
    usiAxisIndex: USINT;
    lrMinPos: LREAL;
    lrMaxPos: LREAL;
    lrMinPwm: LREAL;
    lrMaxPwm: LREAL;
    lrInitPos: LREAL;
END_VAR

;

METHOD CommunicationStateMachine: BOOL
VAR
    x: LREAL;
    pParam: pointer;
END_VAR

IF pAdafruitPWM = 0 THEN
    IF m_pConnector&lt;&gt;0 THEN
        IF  m_pConnector^.pFather &lt;&gt; 0 THEN
            IF  m_pConnector^.pFather^.pFather &lt;&gt; 0 THEN
                pParam := ConfigGetParameter(m_pConnector^.pFather^.pFather, 1);
                IF pParam &lt;&gt; 0 THEN
                    pAdafruitPWM := pParam^.dwDriverSpecific;
                    IF pAdafruitPWM &lt;&gt; 0 THEN
                        usiAxisIndex := pAdafruitPWM^.RegisterAxis();
                    END_IF
                END_IF
            END_IF
        END_IF  
    END_IF
END_IF

IF pAdafruitPwm &lt;&gt; 0 THEN
    bCommunication := pAdafruitPwm^.Operational;
ELSE
    bCommunication := 0;
    SMC_LogFBError(pDrive:=THIS, wID:=300, pbyErrorInstance:=THIS);
    wCommunicationState := 1000;
END_IF

IF NOT bVirtual THEN
    CASE wCommunicationState OF
    0: 
        IF usiAxisIndex &lt;&gt; 255 THEN
            {IF hasattribute (pou: AdafruitPwm, 'setPWMRange')}
                pAdafruitPWM^.setPWMRange(usiAxisIndex, lrMinPwm, lrMaxPwm);
            {END_IF} 
            wCommunicationState := 10;
        END_IF
    10:
        fActPosition := lrInitPos;
        IF bCommunication THEN
            wCommunicationState := 100;
        END_IF
    END_CASE

    // simulate actual values
    IF bRegulatorOn THEN
        IF pAdafruitPWM &gt; 0 THEN
            IF lrMinPos &lt; lrMaxPos THEN
                x := LIMIT(lrMinPos, fSetPosition, lrMaxPos);
            ELSE
                x := LIMIT(lrMaxPos, fSetPosition, lrMinPos);
            END_IF
            pAdafruitPWM^.alrPWM[usiAxisIndex] := (x + fOffsetPosition - lrMinPos) / (lrMaxPos - lrMinPos);
        END_IF

        fActPosition := fSetPosition;
        IF iMovementType = 0 THEN (*modulo*)
            x := SM0.fmod(fActPosition + fOffsetPosition (* + fLogicalOffsetPosition logisch ist nicht gleichzeitig virtuell, oder?*) - fLastActPosition - fLastPositionOffset, fPositionPeriod);
            IF x*2 &gt; fPositionPeriod THEN
                x := x - fPositionPeriod;
            END_IF
            fActVelocity := x / fTaskCycle;
        ELSE
            fActVelocity := (fActPosition + fOffsetPosition (* + fLogicalOffsetPosition logisch ist nicht gleichzeitig virtuell, oder?*) - fLastActPosition - fLastPositionOffset) / fTaskCycle;
        END_IF
        fActAcceleration := (fActVelocity - fLastActVelocity) / fTaskCycle;
        fActJerk := fSetJerk;
        fActTorque := fSetTorque;
    END_IF

    //check limits
    IF nAxisState&lt;&gt;errorstop AND nAxisState&lt;&gt;power_off THEN
        IF fMaxVelocity &gt; 0 AND fMaxVelocity &lt; ABS(fActVelocity) THEN
            SMC_LogFBError(THIS, SMC_VD_MAX_VELOCITY_EXCEEDED, 0);
            iOwner := -1;
            nAxisState := errorstop;
        END_IF      

        IF fActAcceleration*fActVelocity &gt; 0 THEN
            //acceleration
            IF fMaxAcceleration &gt; 0 AND fMaxAcceleration &lt; ABS(fActAcceleration) AND NOT bError THEN
                SMC_LogFBError(THIS, SMC_VD_MAX_ACCELERATION_EXCEEDED, 0);
                iOwner := -1;
                nAxisState := errorstop;
            END_IF      
        ELSE
            //deceleration
            IF fMaxDeceleration &gt; 0 AND fMaxDeceleration &lt; ABS(fActAcceleration) AND NOT bError THEN
                SMC_LogFBError(THIS, SMC_VD_MAX_DECELERATION_EXCEEDED, 0);
                iOwner := -1;
                nAxisState := errorstop;
            END_IF      
        END_IF  
    END_IF
    IF nAxisState=errorstop OR nAxisState=power_off OR bySwitchingState=0 THEN
        fActVelocity := 0;
        fActAcceleration := 0;
    END_IF

    //store old values
    fLastActPosition := fActPosition;
    fLastPositionOffset := fOffsetPosition(* + fLogicalOffsetPosition logisch ist nicht gleichzeitig virtuell, oder?*);
    fLastActVelocity := fActVelocity;

    pAdafruitPWM^.axEnable[usiAxisIndex] := bRegulatorOn;

    bRegulatorRealState := bRegulatorOn AND bCommunication;     (* immitate power/halt states *)
    bDriveStartRealState := bDriveStart;

    IF bErrorAckn THEN
        bError:=FALSE;
        bErrorAckn := FALSE;
    END_IF
END_IF

CommunicationStateMachine := SUPER^.CommunicationStateMachine();

METHOD GetMappedDriveParameter: BOOL
VAR_INPUT
    bRead: BOOL;
    diParameterNumber: DINT;
END_VAR
VAR_OUTPUT
    diDriveParameterNumber: DINT;
    usiDataLength: USINT;
END_VAR

IF diParameterNumber = 1030 OR diParameterNumber = 1031 OR diParameterNumber = 1032 THEN
    diDriveParameterNumber := 0;
    GetMappedDriveParameter:=TRUE;
ELSE
    GetMappedDriveParameter := SUPER^.GetMappedDriveParameter(
        bRead:=bRead, 
        diParameterNumber:=diParameterNumber,
        diDriveParameterNumber =&gt; diDriveParameterNumber,
        usiDataLength =&gt; usiDataLength);
END_IF

METHOD GetStandardConfigParams: BOOL
VAR
    pParam: pointer;
END_VAR

GetStandardConfigParams := SUPER^.GetStandardConfigParams();

IF m_pConnector &lt;&gt; 0 THEN

    pParam := ConfigGetParameter(m_pConnector, 65536);
    IF pParam &lt;&gt; 0 THEN
        lrMinPos := ConfigGetParameterValueLREAL(pParam, 0);
    END_IF

    pParam := ConfigGetParameter(m_pConnector, 65537);
    IF pParam &lt;&gt; 0 THEN
        lrMaxPos := ConfigGetParameterValueLREAL(pParam, 0);
    END_IF

    pParam := ConfigGetParameter(m_pConnector, 65538);
    IF pParam &lt;&gt; 0 THEN
        lrInitPos := ConfigGetParameterValueLREAL(pParam, 0);
    END_IF

    pParam := ConfigGetParameter(m_pConnector, 65539);
    IF pParam &lt;&gt; 0 THEN
        lrMinPwm := ConfigGetParameterValueLREAL(pParam, 0);
    END_IF

    pParam := ConfigGetParameter(m_pConnector, 65540);
    IF pParam &lt;&gt; 0 THEN
        lrMaxPwm := ConfigGetParameterValueLREAL(pParam, 0);
    END_IF
END_IF

byRealControllerMode := SMC_position;

FUNCTION_BLOCK SMC_PosControlInput
VAR_INPUT
    bLimitPos: BOOL;
    bLimitNeg: BOOL;
    wActPosition: WORD;
    dwActPosition: DWORD;
    bExternalError: BOOL;
    bRegulatorRealState: BOOL;
    bDriveStartRealState: BOOL;
    dwEncoderCounterModulo: DWORD;
END_VAR
VAR_OUTPUT
    bUseWord: BOOL;
END_VAR

IF wActPosition &lt;&gt; 0 THEN
    bUseWord := TRUE;
END_IF

FUNCTION_BLOCK SMC_PosControlOutput
VAR_INPUT
    bRegulatorOnIn: BOOL;
    bDriveStartIn: BOOL;
    fSetVelocity: LREAL;
    fMaxVelocityNeg: LREAL;
    fMaxVelocityPos: LREAL;
    diMaxVelocityNeg: DINT;
    diZeroVelocity: DINT;
    diMaxVelocityPos: DINT;
    bInvertDirection: BOOL;
END_VAR
VAR_OUTPUT
    bRegulatorOn: BOOL;
    bDriveStart: BOOL;
    diSetVelocity: DINT;
    iSetVelocity: INT;
END_VAR

bRegulatorOn := bRegulatorOnIn;
bDriveStart := bDriveStartIn;

IF bDriveStartIn THEN
    IF bInvertDirection THEN
        fSetVelocity := fSetVelocity*-1;
    END_IF
    IF fSetVelocity &lt; 0 THEN
        fSetVelocity := MAX(fSetVelocity, fMaxVelocityNeg);
        diSetVelocity := diZeroVelocity + LREAL_TO_DINT(fSetVelocity/fMaxVelocityNeg* DINT_TO_LREAL(diMaxVelocityNeg-diZeroVelocity));
    ELSE
        fSetVelocity := MIN(fSetVelocity, fMaxVelocityPos);
        diSetVelocity := diZeroVelocity + LREAL_TO_DINT(fSetVelocity/fMaxVelocityPos* DINT_TO_LREAL(diMaxVelocityPos-diZeroVelocity));
    END_IF
ELSE
    diSetVelocity := diZeroVelocity;
END_IF

iSetVelocity := DINT_TO_INT(diSetVelocity);