Второй сем / LR6_UTC / script_final

//Глобальные переменные
const int PedalSensorPin = A0; //аналоговый вход с потенциометра педали
const int ThrottleSensorPin = A1; //аналоговый вход с потенциометра заслонки
const int Potenciometer1 = A3; //«П» аналоговые входы с потенциометров,
const int Potenciometer2 = A4; //«И» задающих коэффициенты
const int Potenciometer3 = A5; //«Д» ПИД-регулятора
const int MotorThrottlePin = 6; //выход (с ШИМ) для управл-я мотором заслонки
const float Kpmax = 400; // Макс. значение коэф. ПИД-регулятора Кп
const float Kimax = 0.3; // Макс. значение коэф. ПИД-регулятора Ки
const float Kdmax = 10; // Макс. значение коэф. ПИД-регулятора Кд
const int PedalValueMin = 200; // см. табл. 1
const int PedalValueMax = 760; //см. табл. 1
const int PedalValueRange = PedalValueMax-PedalValueMin; //диапазон изменения значений датчика педали
const int ThrottleValueMin = 820; // см. табл. 1
const int ThrottleValueMax = 74; // см. табл. 1
const int ThrottleValueRange = ThrottleValueMax-ThrottleValueMin; //диапазон изм-я значений датч. заслонки
float eprev = 0.0; //значение ошибки на предыдущем шаге работы регулятора
float ie = 0.0; // интеграл ошибки
unsigned long tprev = 0; //знач-е времени на предыдущ. шаге работы регулятора


void setup() {
Serial.begin(9600);
pinMode(MotorThrottlePin, OUTPUT);
pinMode(PedalSensorPin, INPUT);
pinMode(ThrottleSensorPin, INPUT);
pinMode(Potenciometer1, INPUT);
pinMode(Potenciometer2, INPUT);
pinMode(Potenciometer3, INPUT);
}

void loop() {
//Переменные функции loop()
unsigned long t; //текущее время в мс
double dt; //dt в секундах!
int PedalValue; //оцифрованное напряжение на потенциометре педали
int ThrottleValue; //оцифрованное напряжение на потенциометре заслонки
float g; //задающее воздействие(угол педали)в безразмерном виде
float y; //выходная переменная (угол заслонки) в безразмерном виде
float e; //текущая ошибка
float e1; //ошибка на прошлом шаге
float de_dt; // производная от ошибки de/dt
float u; //сигнал управления
int P1value; //оцифрованное напряжение на потенциометре 1 (0 ... 1023)
int P2value; //оцифрованное напряжение на потенциометре 2 (0 ... 1023)
int P3value; //оцифрованное напряжение на потенциометре 3 (0 ... 1023)
float Kp; //коэффициент Kp ПИД-регулятора
float Ki; //коэффициент Ki ПИД-регулятора
float Kd; //коэффициент Kd ПИД-регулятора

t = micros();
dt = t / 1000;
PedalValue = analogRead(PedalSensorPin);
ThrottleValue = analogRead(ThrottleSensorPin);
g = float((PedalValue - PedalValueMin)) / PedalValueRange;
y = float((ThrottleValue - ThrottleValueMin)) / (ThrottleValueMax-ThrottleValueMin);
e = g - y;
if (abs(e) < 0.01) {
e = 0;
}
ie += (dt - tprev) * e;
de_dt = (e - e1) / (dt - tprev);
tprev = dt;
e1 = e;
P1value = analogRead(Potenciometer1);
P2value = analogRead(Potenciometer2);
P3value = analogRead(Potenciometer3);

Kp = Kpmax * P1value / 1023;
Ki = Kimax * P2value / 1023;
Kd = Kdmax * P3value / 1023;

u = e * Kp + ie * Ki + de_dt * Kd;

if (u < 0) {
u = 0;
}

if (u > 255) {
u = 255;
}

analogWrite(MotorThrottlePin, int(u));

Serial.print("g: ");
Serial.print(g);
Serial.print("\ty: ");
Serial.print(y);
Serial.print("\te: ");
Serial.print(e);
Serial.print("\tu: ");
Serial.print(u);
Serial.print("\tKp: ");
Serial.print(Kp);
Serial.print("\tKi: ");
Serial.print(Ki);
Serial.print("\tKd: ");
Serial.print(Kd);
Serial.print("\tP1value: ");
Serial.print(P1value);
Serial.print("\tP2value: ");
Serial.print(P2value);
Serial.print("\tP3value: ");
Serial.print(P3value);
Serial.print("\n");
delay(100);
}