Raspberry Pi Legacy

FUNCTION_BLOCK AdafruitPwm EXTENDS i2c
VAR_INPUT
    alrPWM: ARRAY [..] OF ;
    axEnable: ARRAY [..] OF ;
END_VAR
VAR
    _freq: UINT;
    _usiSMAxes: USINT;
    _rPeriod: LREAL;
    _alrOldPWM: ARRAY [..] OF ;
    _alrMin: ARRAY [..] OF ;
    _alrMax: ARRAY [..] OF ;
END_VAR
VAR_TEMP
    i: USINT;
    usi: USINT;
END_VAR

SUPER^();

CASE _iState OF
0:
    IF usiAddress = 0 THEN
        usiAddress := 16#40;
    END_IF

    IF SUPER^.init() THEN
        _iState := 5;
    END_IF
5:
    write8(PCA9685_MODE1, 16#10); // go to sleep

    usi := LREAL_TO_USINT(25000000 / 4096.0 / 0.88 (*0.88 after check with oszi*)  / _freq) - 1;
    write8(PCA9685_PRESCALE, usi); // set the prescaler

    _rPeriod := LREAL#1.0 / _freq * 1000; //Periode in ms

    _iState := 6;
6:
    write8(PCA9685_MODE1, 16#A0);  //  This sets the MODE1 register to turn on auto increment.

    FOR i := 0 TO 15 DO
        _alrOldPWM[i] := -1;
    END_FOR

    _iState := 10;
END_CASE

METHOD BeforeWriteOutputs: INT
VAR
    i: USINT;
END_VAR

SUPER^.BeforeWriteOutputs();

IF _iState = 10 THEN
    FOR i:=0 TO 15 DO
        IF alrPWM[i] <> _alrOldPWM[i] AND axEnable[i] > 0 THEN
            setPWM(i, alrPWM[i]);
            _alrOldPWM[i] := alrPWM[i];
        END_IF
    END_FOR 
END_IF

METHOD Initialize: UDINT
VAR_INPUT
    wModuleType: UINT;
    dwInstance: UDINT;
    pConnector: pointer;
END_VAR
VAR
    pParam: pointer;
    udiResult: UDINT;
END_VAR

SUPER^.Initialize(wModuleType, dwInstance, pConnector);

pParam := ConfigGetParameter(_pConnector, 2);
IF pParam <> 0 THEN
    _freq := IoStandard.ConfigGetParameterValueWord(pParam, ADR(udiResult));
END_IF

(* no longer supported
pParam := ConfigGetParameter(_pConnector, 3);
IF pParam <> 0 THEN
    _usiUsedOutputs := IoStandard.ConfigGetParameterValueByte(pParam, ADR(udiResult));
END_IF
*)

pParam := ConfigGetParameter(_pConnector, 1);
IF pParam <> 0 THEN   //store own pointer in this param
    pParam^.dwDriverSpecific := THIS;
END_IF

_usiSMAxes := 0;

METHOD setPWMRaw: 
VAR_INPUT
    usiChannel: USINT;
    ui: UINT;
END_VAR
VAR
    aby: ARRAY [..] OF ;
END_VAR

aby[0] := LED0_ON_L + 4*usiChannel;

aby[3] := UINT_TO_USINT(ui);
aby[4] := UINT_TO_USINT(ui/256);

write(ADR(aby), 5);

METHOD setPWM: 
VAR_INPUT
    usiChannel: USINT;
    rValue: LREAL;
END_VAR
VAR
    aby: ARRAY [..] OF ;
    ui: UINT;
END_VAR

aby[0] := LED0_ON_L + 4 * usiChannel;

ui := LREAL_TO_UINT((_alrMin[usiChannel] + LIMIT(0, rValue, 1) * (_alrMax[usiChannel] - _alrMin[usiChannel])) / _rPeriod * 4096 - 1);
aby[3] := UINT_TO_USINT(ui);
aby[4] := UINT_TO_USINT(ui/256);

write(ADR(aby), 5);

METHOD RegisterAxis: USINT

IF _usiSMAxes < 16 THEN
    RegisterAxis := _usiSMAxes;
    _usiSMAxes := _usiSMAxes + 1;
ELSE
    RegisterAxis := 255;
END_IF

METHOD setPWMRange: 
VAR_INPUT
    usiChannel: USINT;
    alrMin: LREAL;
    alrMax: LREAL;
END_VAR

IF usiChannel < 16 THEN
    _alrMin[usiChannel] := alrMin;
    _alrMax[usiChannel] := alrMax;
END_IF