uint32_t error[3];
    uint32_t A[3];
    uint32_t
    uint8_t Kp = 1;
    uint8_t Ki = 1;
    uint8_t Kd = 1;
    const
    A[0] =  Kp + Ki*dt + Kd/dt
    A[1] = -Kp - 2*Kd/dt
    A[2] = Kd/dt
    error[2] := 0 // e(t-2)
    error[1] := 0 // e(t-1)
    error[0] := 0 // e(t)
    CT_UART.THR = 'S';
    CT_UART.THR = 'N';
    CT_UART.THR = '\r';
    reference_count = position // initil reading from decoder as unit32
    while (finished = false) {
        error[2] := error[1]
        error[1] := error[0]
        error[0] := (setpoint+reference_count) − measured_value // currentin reading from decoder
        output := output + A[0] * error[0] + A[1] * error[1] + A[2] * error[2]
        sleep(dt)

        if (time +dt > period ){
            refrence_count = position // current reading from decoder.
        }
    }