Кибернетика, автоматика, протоколы

Вот реалный пример как это сделать на Ц. 

Причесал свертку и взял ПИД из ссылки.

Это модель

#ifndef MODEL_H_
#define MODEL_H_
#include <math.h>
struct convolve_params{
   float* h;
   size_t size;
   float* input;
   float* output;
};
void set_aperiodic_weight(float T, float sample_period, size_t size, convolve_params* cpar);
void set_square_weight(float ampl, size_t size, convolve_params* cpar);
float convolve(float x, convolve_params* cpar);
#endif  // MODEL_H_

#include "model.h"

void set_aperiodic_weight(float T, float sample_period, size_t size, convolve_params* cpar) {
   cpar->h = (float*)malloc(size*sizeof(float));
   cpar->input = (float*)malloc(size*sizeof(float));
   cpar->output = (float*)malloc(size*sizeof(float));
   cpar->size = size;
   float t = 0.0;
   for (size_t i = 0;i<size; i++) {
      cpar->h[i] = 5/T *(exp(-t/T));
      cpar->input[i] = 0.0;
      t += sample_period;
   }
}

void set_square_weight(float ampl, size_t size, convolve_params* cpar) {
   cpar->h = (float*)malloc(size*sizeof(float));
   cpar->input = (float*)malloc(size*sizeof(float));
   cpar->output = (float*)malloc(size*sizeof(float));
   cpar->size = size;
   for (size_t i = 0;i<size; i++) {
      cpar->h[i] = ampl;
      cpar->input[i] = 0.0;
   }
}

float convolve(float x, convolve_params* cpar) {
   for (size_t i = cpar->size-1; i>0; i--){
      cpar->input[i] = cpar->input[i-1];
   }
   cpar->input[0] = x;
   float integral = 0;
   for (size_t i = 0; i<cpar->size; i++){
      float step = cpar->h[i]*cpar->input[i];
      cpar->output[i] = integral + step;
      integral += step;
   }
   return (cpar->output[cpar->size-1] / cpar->size);
}

Модель можно проверить на ответ

static void get_responce(convolve_params* p) {
   for (size_t i=0;i<20;i++) {
      std::cout << convolve(0, p) << std::endl;
   }
   for (size_t i=0;i<50;i++) {
      std::cout << convolve(1, p) << std::endl;
   }
   for (size_t i=0;i<100;i++) {
      std::cout << convolve(0, p) << std::endl;
   }
}

....

   convolve_params p;
   set_aperiodic_weight(1, 0.1, 50, &p);
   //set_square_weight(1, 50, &p);
   get_responce(&p);

А вот ПИД + модель. Параметры од балды но тут хотел показать как работает.

struct pid_params{
   float ITerm, lastInput;
   float kp, ki, kd;
   float outMin, outMax;
};
float PIDCompute(float Input, float Setpoint, pid_params* pidp )
{
   float Output;
   /*Compute all the working error variables*/
   float error = Setpoint - Input;
   pidp->ITerm+= (pidp->ki * error);
   if(pidp->ITerm> pidp->outMax) pidp->ITerm= pidp->outMax;
   else if(pidp->ITerm< pidp->outMin) pidp->ITerm= pidp->outMin;
   float dInput = (Input - pidp->lastInput);
   /*Compute PID Output*/
   Output = pidp->kp * error + pidp->ITerm- pidp->kd * dInput;
   if(Output> pidp->outMax) Output = pidp->outMax;
   else if(Output < pidp->outMin) Output = pidp->outMin;
   /*Remember some variables for next time*/
   pidp->lastInput = Input;
   return Output;
}
int main(int argc, char *argv[])
{
   convolve_params p;
   pid_params pidp;
   pidp.ITerm = 0;
   pidp.lastInput = 0;
   pidp.kp = 0.1;
   pidp.ki = 3;
   pidp.kd = 0;
   pidp.outMin = 0;
   pidp.outMax = 10.0;
   set_aperiodic_weight(1, 0.1, 50, &p);

   float responce = 0;
   float pid_out = 0;
   for (size_t i =0 ; i < 1000; i++) {
      responce = convolve(pid_out, &p);
      pid_out = PIDCompute(responce,0.5,&pidp);
      std::cout << responce << "," << pid_out << std::endl;
   }
   return 0;
}

Так что без теории никак.