PIDC.h

/*
 * Author: Napattnpp
 * File: PIDC.h
 * Version: p.7.1
*/

class PIDC {
    private:
        unsigned n = 0;
        unsigned last = 0;
        unsigned pre = 0;
        unsigned s = 0;
        unsigned char i_char = 0;
        unsigned char i_str = 0;

        // Tune
        int arrow = 0;
        TYPE knobValue = 0;
        TYPE pre_knobValue = 0;

        void update_errorState(unsigned n);

    public:
        Sensor sen;

        unsigned char NOS = 0;

        char errorState[16][16];
        int errorList[16];

        int error = 0;
        int pre_error = 0;
        int sum_error = 0;

        TYPE kp = 0;
        TYPE ki = 0;
        TYPE kd = 0;
        TYPE result = 0;

        void init(unsigned char _NOS);
        void set_errorList(...);
        int comp(char str1[], char str2[]);

        // Get error
        void gett_error();

        // Calulate the error state
        void ces();
        void start(void (*func)());
        void tune(void (*func)(), int newMaxValue, bool executeState);
};

void PIDC::update_errorState(unsigned n) {
    // Convert to binary
    i_char = 0;

    for (unsigned i = 1 << (NOS - 1); i > 0; i = i >> 1) {
        errorState[i_str][i_char] = (n & i) ? '1' : '0';
        i_char++;
    }
    i_str++;
}

int PIDC::comp(char str1[], char str2[]) {
  int acl = 0;
  for (int i = 0; i < NOS; i++) {
    if (str1[i] == str2[i]) {
      acl++;
    } else {
      acl--;
      if (acl < 0) { break; }
    }
  }

  if (acl == NOS) {
    return 1;
  } else {
    return 0;
  }
}

void PIDC::init(unsigned char _NOS) {
    NOS = _NOS;
}

void PIDC::ces() {
    for (unsigned cur = 1; cur != (1 << NOS); cur = cur << 1) {
        if (s) {
            pre = (last | cur);
            update_errorState(pre);
        } else {
            s = !s;
        }
        update_errorState(cur);

        last = cur;
    }
}

void PIDC::set_errorList(...) {
    int _NOS = ((NOS*2) - 1);
    va_list vaList;

    va_start(vaList, _NOS);
    for (unsigned char i = 0; i < _NOS; i++) {
        errorList[i] = va_arg(vaList, int);
    }
    va_end(vaList);
} 

void PIDC::gett_error() {
    // Get current error state
    sen.conv_str();

    /*
     * Compare current error state with seted error state
     * and set a error value
    */
    for (unsigned char i = 0; i < ((NOS*2) - 1); i++) {
        if (comp(sen.val_str, errorState[i])) {
            error = errorList[i];
            break;
        }
    }
}

void PIDC::start(void (*func)()) {
    gett_error();
    result = ((kp*error) + (ki*sum_error) + (kd*(error-pre_error)));

    (func)();

    pre_error = error;
    sum_error += error;
}

void PIDC::tune(void (*func)(), int newMaxValue, bool executeState) {
  // Show default PID value
  SCREEN_CLEAR

  SCREEN_TEXT(0, 0, "[Tune PID]")
  SCREEN_TEXT(1, 4, "kp:")
  SCREEN_TEXT(2, 4, "ki:")
  SCREEN_TEXT(3, 4, "kd:")
  SCREEN_SHOW

  while (executeState) {
    // If button is pressed mode will change
    if (sw_ok()) {
        arrow++;
        if (arrow != 1) { SCREEN_TEXT(arrow-1, 0, "   ") SCREEN_SHOW }
        if (arrow == 5) { arrow = 1; }

        SCREEN_TEXT(arrow, 0, "-->")
        SCREEN_SHOW

        delay(300);
    }

    knobValue = knob();
    knobValue = map(knobValue, 0, MAX_ANALOG, 0, newMaxValue);

    // Update PID value and screen
    // If mode is same don't update screen
    if (pre_knobValue != knobValue) {
        if (arrow == 1) {
            kp = knobValue;
            SCREEN_TEXT(1, 8, PFM, kp)    SCREEN_SHOW
        } else if (arrow == 2) {
            ki = knobValue;
            SCREEN_TEXT(2, 8, PFM, ki)    SCREEN_SHOW
        } else if (arrow == 3) {
            kd = knobValue;
            SCREEN_TEXT(3, 8, PFM, kd)    SCREEN_SHOW
        }
    }

    if (arrow == 4) {
        start(func);
    } else {
        AO();
    }

    pre_knobValue = knobValue;
  }
}