Средства и методы разработки

Жесть-то какая! Зачем на ублюдской змеюке-то? 

Я на С попросил, он дал очень даже вменяемый код:

#include <stdio.h>
#include <time.h>

// PID structure to hold controller state
typedef struct {
    double Kp;  // Proportional gain
    double Ki;  // Integral gain
    double Kd;  // Derivative gain
    double setpoint;  // Desired value
    double integral;  // Integral term
    double previous_error;  // Previous error for derivative term
    clock_t last_time;  // Last time stamp for time delta calculation
} PIDController;

// Initialize PID controller
void PID_Init(PIDController *pid, double Kp, double Ki, double Kd, double setpoint) {
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->setpoint = setpoint;
    pid->integral = 0.0;
    pid->previous_error = 0.0;
    pid->last_time = clock();
}

// Compute PID output
double PID_Compute(PIDController *pid, double measured_value) {
    // Calculate time difference
    clock_t now = clock();
    double time_diff = (double)(now - pid->last_time) / CLOCKS_PER_SEC;
    pid->last_time = now;

    // Calculate error
    double error = pid->setpoint - measured_value;

    // Proportional term
    double Pout = pid->Kp * error;

    // Integral term
    pid->integral += error * time_diff;
    double Iout = pid->Ki * pid->integral;

    // Derivative term
    double derivative = (error - pid->previous_error) / time_diff;
    double Dout = pid->Kd * derivative;

    // Save error for next derivative calculation
    pid->previous_error = error;

    // Calculate total output
    double output = Pout + Iout + Dout;

    return output;
}

int main() {
    // Example usage
    PIDController pid;
    double setpoint = 100.0;  // Desired value
    double measured_value = 0.0;  // Initial measured value

    // Initialize PID controller with gains and setpoint
    PID_Init(&pid, 2.0, 0.5, 1.0, setpoint);

    // Simulate control loop
    for (int i = 0; i < 100; i++) {
        // Compute PID output
        double output = PID_Compute(&pid, measured_value);

        // Simulate system response (e.g., update measured_value)
        measured_value += output * 0.1;  // Example system dynamics

        // Print results
        printf("Iteration %d: Setpoint = %.2f, Measured = %.2f, Output = %.2f\n",
               i, setpoint, measured_value, output);

        // Simulate delay (e.g., for real-time systems)
        // sleep(1);  // Uncomment if running in a real-time environment
    }

    return 0;
}

Вот хоть прямо так бери куски, и используй. Хоть на ПК, хоть на МК…

[code]

test

[/code]