🧮 ПИД регулирование и точное управление. Математика правит железом!

 1🧮 PID = Proportional + Integral + Derivative
 2
 3ОСНОВНАЯ ФОРМУЛА (ЗАПОМНИТЬ НАВСЕГДА!):
 4Output = Kp × Error + Ki × ∫Error dt + Kd × dError/dt
 5
 6ГДЕ:
 7• Error = Setpoint - Current_Value (желаемое - фактическое)
 8• Kp, Ki, Kd = коэффициенты настройки (ИСКУССТВО!)
 9
10🎯 P - PROPORTIONAL (Пропорциональная часть):
11P_output = Kp × Error
12
13ЧТО ДЕЛАЕТ:
14• Реагирует НА РАЗМЕР ошибки
15• Большая ошибка → большая коррекция
16• Маленькая ошибка → маленькая коррекция
17
18ПОВЕДЕНИЕ:
19• Kp слишком мал → МЕДЛЕННАЯ реакция
20• Kp слишком велик → OSCILLATIONS (колебания)
21• Kp в самый раз → быстрая but stable коррекция
22
23ПРИМЕР: Управление скоростью автомобиля
24Error = 60 km/h - 50 km/h = 10 km/h
25P_output = 0.5 × 10 = 5 (добавить газу)
26
27🔄 I - INTEGRAL (Интегральная часть):
28I_output = Ki × ∫Error dt (сумма всех ошибок за время)
29
30ЧТО ДЕЛАЕТ:
31• НАКАПЛИВАЕТ ошибки за время
32• Устраняет ПОСТОЯННУЮ ошибку (steady-state error)
33• "Память" системы
34
35ПОВЕДЕНИЕ:
36• Ki = 0 → остается постоянная ошибка
37• Ki слишком велик → система становится НЕСТАБИЛЬНОЙ
38• Ki правильный → ИДЕАЛЬНОЕ попадание в цель
39
40ПРИМЕР: Термостат
41Если температура ВСЕГДА на 1°C ниже → I накапливает эту ошибку → увеличивает мощность нагрева
42
43📈 D - DERIVATIVE (Дифференциальная часть):
44D_output = Kd × dError/dt (скорость изменения ошибки)
45
46ЧТО ДЕЛАЕТ:
47• Реагирует на СКОРОСТЬ изменения
48• ПРЕДСКАЗЫВАЕТ будущие ошибки
49• ДЕМПФИРУЕТ колебания
50
51ПОВЕДЕНИЕ:
52• Kd = 0 → может быть overshoot
53• Kd слишком велик → система становится SLUGGISH
54• Kd правильный → SMOOTH приближение к цели
55
56ПРИМЕР: Автомобильные тормоза ABS
57Колесо начинает блокироваться (быстрое изменение) → D компонент МГНОВЕННО уменьшает тормозное усилие

 1🎛️ TUNING ПИД - КАК НАСТРОИТЬ ИДЕАЛЬНО
 2
 3МЕТОДЫ НАСТРОЙКИ:
 4
 5🔧 MANUAL TUNING (Ручная настройка):
 61. Поставить Ki = 0, Kd = 0
 72. Увеличивать Kp пока система не начнет КОЛЕБАТЬСЯ
 83. Уменьшить Kp в 2 раза
 94. Добавлять Ki пока не исчезнет steady-state error
105. Добавлять Kd для уменьшения overshoot
11
12⚡ ZIEGLER-NICHOLS METHOD:
131. Найти критический Kp (начало колебаний)
142. Измерить период колебаний
153. Рассчитать по формулам:
16   • Kp = 0.6 × Kp_critical
17   • Ki = 2 × Kp / Period
18   • Kd = Kp × Period / 8
19
20🤖 AUTO-TUNING (Автоматическая настройка):
21• Система САМА находит оптимальные коэффициенты
22• Genetic algorithms
23• Machine learning optimization
24• Adaptive control
25
26ТИПИЧНЫЕ ПРОБЛЕМЫ:
27
28🔴 OVERSHOOT (Перерегулирование):
29Симптомы: Система "проскакивает" цель
30Причина: Kp слишком большой или Kd слишком маленький
31Лечение: ↓Kp или ↑Kd
32
33🟡 OSCILLATIONS (Колебания):
34Симптомы: Система качается вокруг цели
35Причина: Kp слишком большой, Ki слишком большой
36Лечение: ↓Kp, ↓Ki, ↑Kd
37
38🟢 SLOW RESPONSE (Медленная реакция):
39Симптомы: Система ДОЛГО достигает цели
40Причина: Все коэффициенты слишком маленькие
41Лечение: ↑Kp (осторожно!)
42
43🔵 STEADY-STATE ERROR (Постоянная ошибка):
44Симптомы: Система НЕ ДОХОДИТ до цели
45Причина: Ki = 0 или Ki слишком маленький
46Лечение: ↑Ki
47
48🆕 ADVANCED PID FEATURES:
49
50ANTI-WINDUP:
51• Проблема: I компонент накапливается до ОГРОМНЫХ значений
52• Решение: Ограничить интеграл разумными пределами
53
54DERIVATIVE KICK:
55• Проблема: Изменение setpoint вызывает СКАЧОК D компонента  
56• Решение: Считать derivative только от process variable
57
58BUMPLESS TRANSFER:
59• Проблема: Переключение между manual/auto вызывает СКАЧКИ
60• Решение: Плавный переход между режимами

 1🏭 ADVANCED CONTROL SYSTEMS
 2
 3CASCADE CONTROL (Каскадное управление):
 4Outer Loop: Temperature Control
 5    ↓ (setpoint)
 6Inner Loop: Heater Power Control
 7    ↓ (actual heating)
 8Process: Temperature
 9
10ПРИМЕР: Точное управление температурой
11• Внешний ПИД: Температура → желаемая мощность нагревателя
12• Внутренний ПИД: Мощность нагревателя → PWM сигнал
13• Результат: БОЛЕЕ ТОЧНАЯ температура
14
15FEEDFORWARD CONTROL:
16• Обычный ПИД: реагирует ПОСЛЕ появления ошибки  
17• Feedforward: ПРЕДСКАЗЫВАЕТ возмущения и компенсирует ДО появления ошибки
18
19ПРИМЕР: Умный термостат
20• Обычный: температура упала → включить нагрев
21• Feedforward: открылось окно → ЗАРАНЕЕ увеличить нагрев
22
23ADAPTIVE PID:
24• Коэффициенты Kp, Ki, Kd АВТОМАТИЧЕСКИ изменяются
25• Система УЧИТСЯ на своих ошибках
26• Подстраивается под изменения в процессе
27
28🤖 MACHINE LEARNING INTEGRATION:
29
30NEURAL NETWORK PID:
31• Neural network предсказывает оптимальные коэффициенты
32• Обучается на historical data
33• Адаптируется к новым условиям
34
35GENETIC ALGORITHM TUNING:
36• "Эволюция" коэффициентов ПИД
37• Симуляция тысяч вариантов
38• Выживают только ЛУЧШИЕ настройки
39
40FUZZY LOGIC CONTROL:
41• Вместо точных чисел → "примерно", "много", "быстро"
42• Более похоже на человеческое мышление
43• Хорошо для сложных нелинейных систем
44
45🔮 PREDICTIVE CONTROL:
46• Система строит МОДЕЛЬ будущего
47• Оптимизирует управление на несколько шагов вперед
48• Учитывает ограничения и constraints
49
50ROBUST CONTROL:
51• Система работает даже при НЕТОЧНОЙ модели
52• Гарантированная стабильность
53• Учитывает uncertainties и disturbances
54
55🆕 DIGITAL TWIN INTEGRATION:
56• Виртуальная копия физической системы
57• Real-time синхронизация
58• Тестирование новых алгоритмов в simulation
59• Predictive maintenance

  1🌡️ HARDWARE SETUP:
  2• ESP32 (floating point math!)
  3• DS18B20 temperature sensor (±0.5°C precision)
  4• Heating element (resistor 10W или Peltier)
  5• MOSFET module для PWM power control
  6• Cooling fan для disturbance simulation
  7• OLED display для real-time data
  8
  9ПОДКЛЮЧЕНИЕ ТЕПЛОВОГО КОНТРОЛЯ:
 10DS18B20:     Data→GPIO4, VCC→3.3V, GND→GND (pullup 4.7kΩ)
 11Heater:      MOSFET→GPIO25, VCC→12V, GND→GND
 12Fan:         MOSFET→GPIO26, VCC→12V, GND→GND
 13OLED:        SDA→GPIO21, SCL→GPIO22
 14
 15#include <OneWire.h>
 16#include <DallasTemperature.h>
 17#include <PIDController.h>
 18
 19// PID Configuration
 20double setpoint = 50.0;    // Target temperature °C
 21double input, output;
 22double Kp = 2.0, Ki = 5.0, Kd = 1.0;
 23
 24PIDController pid(Kp, Ki, Kd);
 25
 26// Advanced PID with anti-windup
 27class AdvancedPID {
 28private:
 29  double kp, ki, kd;
 30  double integral = 0;
 31  double previousError = 0;
 32  double integralMax = 255;  // Anti-windup limit
 33  unsigned long lastTime = 0;
 34  
 35public:
 36  AdvancedPID(double p, double i, double d) : kp(p), ki(i), kd(d) {}
 37  
 38  double compute(double setpoint, double input) {
 39    unsigned long now = millis();
 40    double timeChange = (now - lastTime) / 1000.0; // Convert to seconds
 41    
 42    if (timeChange <= 0) return output; // Prevent division by zero
 43    
 44    // Calculate error
 45    double error = setpoint - input;
 46    
 47    // Proportional term
 48    double proportional = kp * error;
 49    
 50    // Integral term with anti-windup
 51    integral += error * timeChange;
 52    if (integral > integralMax) integral = integralMax;
 53    else if (integral < -integralMax) integral = -integralMax;
 54    double integralTerm = ki * integral;
 55    
 56    // Derivative term (derivative on measurement to avoid derivative kick)
 57    static double lastInput = input;
 58    double derivative = -(input - lastInput) / timeChange;
 59    double derivativeTerm = kd * derivative;
 60    
 61    // Calculate output
 62    output = proportional + integralTerm + derivativeTerm;
 63    
 64    // Clamp output to valid PWM range
 65    output = constrain(output, 0, 255);
 66    
 67    // Remember values for next time
 68    lastInput = input;
 69    lastTime = now;
 70    
 71    return output;
 72  }
 73  
 74  // 🆕 Auto-tuning using relay method
 75  void autoTune(double targetTemp) {
 76    Serial.println("Starting auto-tune...");
 77    
 78    // Relay feedback test
 79    bool relayState = false;
 80    double relayAmplitude = 50; // PWM units
 81    unsigned long switchTime = 0;
 82    double maxTemp = -999, minTemp = 999;
 83    
 84    for (int cycle = 0; cycle < 3; cycle++) {
 85      // Run relay test for several cycles
 86      while (millis() - switchTime < 30000) { // 30 second cycles
 87        double currentTemp = sensors.getTempCByIndex(0);
 88        
 89        if (relayState && currentTemp > targetTemp) {
 90          relayState = false;
 91          switchTime = millis();
 92        } else if (!relayState && currentTemp < targetTemp) {
 93          relayState = true;
 94          switchTime = millis();
 95        }
 96        
 97        analogWrite(HEATER_PIN, relayState ? relayAmplitude : 0);
 98        
 99        // Track oscillation amplitude
100        if (currentTemp > maxTemp) maxTemp = currentTemp;
101        if (currentTemp < minTemp) minTemp = currentTemp;
102        
103        delay(100);
104      }
105    }
106    
107    // Calculate Ziegler-Nichols parameters
108    double amplitude = maxTemp - minTemp;
109    double period = 60.0; // Approximate from cycles
110    
111    kp = 0.6 * (4 * relayAmplitude) / (PI * amplitude);
112    ki = 2 * kp / period;
113    kd = kp * period / 8;
114    
115    Serial.printf("Auto-tuned: Kp=%.2f, Ki=%.2f, Kd=%.2f\n", kp, ki, kd);
116  }
117};
118
119void loop() {
120  // Read temperature
121  sensors.requestTemperatures();
122  double currentTemp = sensors.getTempCByIndex(0);
123  
124  // Compute PID output
125  double heaterPWM = pid.compute(setpoint, currentTemp);
126  
127  // Apply to heater
128  analogWrite(HEATER_PIN, heaterPWM);
129  
130  // 🆕 Disturbance simulation
131  static unsigned long lastDisturbance = 0;
132  if (millis() - lastDisturbance > 30000) { // Every 30 seconds
133    digitalWrite(FAN_PIN, !digitalRead(FAN_PIN)); // Toggle cooling
134    lastDisturbance = millis();
135  }
136  
137  // Data logging for analysis
138  logTemperatureData(currentTemp, setpoint, heaterPWM);
139  
140  // MQTT telemetry
141  publishThermostatData(currentTemp, setpoint, heaterPWM, Kp, Ki, Kd);
142  
143  delay(100); // 10 Hz control loop
144}
145
146// 🆕 Performance analysis
147void analyzePerformance() {
148  // Calculate metrics
149  double settlingTime = calculateSettlingTime();
150  double overshoot = calculateOvershoot();
151  double steadyStateError = calculateSteadyStateError();
152  double riseTime = calculateRiseTime();
153  
154  Serial.println("=== PID PERFORMANCE ANALYSIS ===");
155  Serial.printf("Settling Time: %.2f seconds\n", settlingTime);
156  Serial.printf("Overshoot: %.2f%%\n", overshoot);
157  Serial.printf("Steady State Error: %.2f°C\n", steadyStateError);
158  Serial.printf("Rise Time: %.2f seconds\n", riseTime);
159  
160  // Publish to dashboard
161  publishPerformanceMetrics(settlingTime, overshoot, steadyStateError, riseTime);
162}
163
164ПЕРВАЯ БОЕВАЯ ЗАДАЧА:
165✓ Достичь 50°C с точностью ±0.5°C
166✓ Settling time < 60 секунд
167✓ Overshoot < 10%
168✓ Устойчивость к внешним возмущениям (вентилятор)
169✓ Auto-tuning коэффициентов
170✓ MQTT real-time monitoring

  1🎯 HARDWARE SETUP:
  2• Arduino Uno
  3• High-torque servo с encoder feedback
  4• Potentiometer для setpoint
  5• Load simulation (weights)
  6• Force sensor для disturbance measurement
  7• LCD display для PID parameters
  8
  9ПОДКЛЮЧЕНИЕ ПОЗИЦИОННОГО КОНТРОЛЯ:
 10Servo:           Signal→Pin 9, VCC→6V, GND→GND
 11Encoder:         A→Pin 2, B→Pin 3 (interrupts)
 12Setpoint Pot:    Wiper→A0, VCC→5V, GND→GND
 13Force Sensor:    Signal→A1, VCC→5V, GND→GND
 14Load Motor:      PWM→Pin 6 (simulate varying load)
 15
 16#include <Servo.h>
 17#include <Encoder.h>
 18
 19Servo positionServo;
 20Encoder positionEncoder(2, 3);
 21
 22class ServoPositionPID {
 23private:
 24  double kp = 1.5, ki = 0.1, kd = 0.05;
 25  double integral = 0, lastError = 0;
 26  unsigned long lastTime = 0;
 27  
 28public:
 29  double compute(double targetPosition, double currentPosition) {
 30    unsigned long now = millis();
 31    double dt = (now - lastTime) / 1000.0;
 32    
 33    if (dt <= 0) return 0;
 34    
 35    double error = targetPosition - currentPosition;
 36    
 37    // Proportional
 38    double P = kp * error;
 39    
 40    // Integral with windup protection
 41    integral += error * dt;
 42    integral = constrain(integral, -50, 50); // Prevent windup
 43    double I = ki * integral;
 44    
 45    // Derivative
 46    double derivative = (error - lastError) / dt;
 47    double D = kd * derivative;
 48    
 49    double output = P + I + D;
 50    
 51    lastError = error;
 52    lastTime = now;
 53    
 54    return constrain(output, -90, 90); // Servo angle limits
 55  }
 56  
 57  // 🆕 Adaptive gain scheduling
 58  void adaptGains(double error, double load) {
 59    // Adjust gains based on error magnitude and load
 60    if (abs(error) > 30) {
 61      // Large error: increase proportional gain
 62      kp = 2.5;
 63      ki = 0.05; // Reduce integral to prevent overshoot
 64    } else if (abs(error) < 5) {
 65      // Small error: increase integral for precision
 66      kp = 1.0;
 67      ki = 0.2;
 68    }
 69    
 70    // Adjust for load variations
 71    if (load > 500) { // Heavy load detected
 72      kp *= 1.5; // Increase gains to overcome load
 73      ki *= 1.2;
 74    }
 75  }
 76};
 77
 78ServoPositionPID pidController;
 79
 80void loop() {
 81  // Read current position from encoder
 82  double currentAngle = positionEncoder.read() * 360.0 / 2048.0; // Convert to degrees
 83  
 84  // Read desired position from potentiometer  
 85  double targetAngle = map(analogRead(A0), 0, 1023, 0, 180);
 86  
 87  // Read load/disturbance
 88  double currentLoad = analogRead(A1);
 89  
 90  // Compute PID correction
 91  double correction = pidController.compute(targetAngle, currentAngle);
 92  
 93  // Apply correction to servo
 94  double servoCommand = targetAngle + correction;
 95  positionServo.write(constrain(servoCommand, 0, 180));
 96  
 97  // 🆕 Simulate external disturbances
 98  static unsigned long lastDisturbance = 0;
 99  if (millis() - lastDisturbance > 5000) {
100    // Apply random load disturbance
101    int disturbanceLoad = random(0, 255);
102    analogWrite(6, disturbanceLoad); // Load motor
103    lastDisturbance = millis();
104  }
105  
106  // Performance monitoring
107  double error = abs(targetAngle - currentAngle);
108  if (error < 1.0) {
109    positionAccuracy++;
110  }
111  
112  // Display and log
113  updateDisplay(targetAngle, currentAngle, correction);
114  logPositionData(targetAngle, currentAngle, correction, currentLoad);
115  
116  delay(20); // 50 Hz control loop
117}
118
119ПЕРВАЯ БОЕВАЯ ЗАДАЧА:
120✓ Position accuracy ±1 degree
121✓ Response time < 0.5 seconds  
122✓ No overshoot при step input
123✓ Устойчивость к external loads
124✓ Adaptive gain adjustment
125✓ Performance metrics analysis

  1🏎️ HARDWARE SETUP:
  2• ESP32 (для high-frequency PWM)
  3• DC Motor с optical encoder
  4• L298N motor driver
  5• Tachometer (IR sensor + encoder disk)
  6• Variable load (brake mechanism)
  7• Current sensor (ACS712)
  8
  9ПОДКЛЮЧЕНИЕ СКОРОСТНОГО КОНТРОЛЯ:
 10Motor Driver:    IN1→GPIO16, IN2→GPIO17, ENA→GPIO25
 11Encoder:         A→GPIO18, B→GPIO19 (interrupts)
 12Current Sensor:  Signal→GPIO34 (ADC)
 13Brake Servo:     Signal→GPIO26
 14Tachometer:      Signal→GPIO35
 15
 16#include <ESP32Encoder.h>
 17
 18ESP32Encoder speedEncoder;
 19
 20class MotorSpeedPID {
 21private:
 22  double kp = 0.8, ki = 2.0, kd = 0.01;
 23  double integral = 0, lastRPM = 0;
 24  unsigned long lastTime = 0;
 25  double outputFilter = 0; // Low-pass filter для output
 26  
 27public:
 28  double compute(double targetRPM, double currentRPM) {
 29    unsigned long now = millis();
 30    double dt = (now - lastTime) / 1000.0;
 31    
 32    if (dt <= 0) return outputFilter;
 33    
 34    double error = targetRPM - currentRPM;
 35    
 36    // Proportional
 37    double P = kp * error;
 38    
 39    // Integral with conditional integration
 40    if (abs(error) < 50) { // Only integrate when close to target
 41      integral += error * dt;
 42      integral = constrain(integral, -100, 100);
 43    }
 44    double I = ki * integral;
 45    
 46    // Derivative on measurement (avoid derivative kick)
 47    double derivative = -(currentRPM - lastRPM) / dt;
 48    double D = kd * derivative;
 49    
 50    double rawOutput = P + I + D;
 51    
 52    // Low-pass filter на output для smooth operation
 53    double alpha = 0.8; // Filter coefficient
 54    outputFilter = alpha * outputFilter + (1 - alpha) * rawOutput;
 55    
 56    lastRPM = currentRPM;
 57    lastTime = now;
 58    
 59    return constrain(outputFilter, -255, 255);
 60  }
 61  
 62  // 🆕 Feed-forward compensation
 63  double feedForward(double targetRPM, double loadCurrent) {
 64    // Compensate for known load characteristics
 65    double baseOutput = targetRPM * 0.1; // Basic RPM to PWM mapping
 66    double loadCompensation = loadCurrent * 10; // Current-based load comp
 67    
 68    return baseOutput + loadCompensation;
 69  }
 70  
 71  // 🆕 Cascade control с current loop
 72  double cascadeControl(double targetRPM, double currentRPM, double targetCurrent, double actualCurrent) {
 73    // Outer loop: Speed control
 74    double speedError = targetRPM - currentRPM;
 75    double desiredCurrent = speedPID.compute(targetRPM, currentRPM);
 76    
 77    // Inner loop: Current control (faster dynamics)
 78    double currentError = desiredCurrent - actualCurrent;
 79    double pwmOutput = currentPID.compute(desiredCurrent, actualCurrent);
 80    
 81    return pwmOutput;
 82  }
 83};
 84
 85MotorSpeedPID speedController;
 86
 87volatile long encoderCount = 0;
 88double currentRPM = 0;
 89
 90void IRAM_ATTR encoderISR() {
 91  encoderCount++;
 92}
 93
 94void loop() {
 95  // Calculate RPM from encoder
 96  static unsigned long lastRPMCalculation = 0;
 97  static long lastEncoderCount = 0;
 98  
 99  if (millis() - lastRPMCalculation >= 100) { // Calculate every 100ms
100    long deltaCount = encoderCount - lastEncoderCount;
101    double deltaTime = (millis() - lastRPMCalculation) / 1000.0;
102    
103    // RPM = (pulses per revolution × 60) / (time in seconds × pulses per revolution)
104    currentRPM = (deltaCount * 60.0) / (deltaTime * ENCODER_PPR);
105    
106    lastEncoderCount = encoderCount;
107    lastRPMCalculation = millis();
108  }
109  
110  // Read target speed from potentiometer или MQTT
111  double targetRPM = map(analogRead(A0), 0, 4095, 0, 3000); // 0-3000 RPM range
112  
113  // Read motor current для load detection
114  double motorCurrent = (analogRead(34) - 2048) * 5.0 / 4096.0; // Convert to amps
115  
116  // Compute PID output
117  double pidOutput = speedController.compute(targetRPM, currentRPM);
118  
119  // Add feed-forward compensation
120  double ffOutput = speedController.feedForward(targetRPM, motorCurrent);
121  double totalOutput = pidOutput + ffOutput;
122  
123  // Apply to motor
124  if (totalOutput >= 0) {
125    digitalWrite(16, HIGH);
126    digitalWrite(17, LOW);
127    analogWrite(25, constrain(totalOutput, 0, 255));
128  } else {
129    digitalWrite(16, LOW);
130    digitalWrite(17, HIGH);
131    analogWrite(25, constrain(-totalOutput, 0, 255));
132  }
133  
134  // 🆕 Dynamic load testing
135  static unsigned long lastLoadChange = 0;
136  if (millis() - lastLoadChange > 10000) { // Every 10 seconds
137    int brakePosition = random(0, 90); // Random brake load
138    brakeServo.write(brakePosition);
139    lastLoadChange = millis();
140  }
141  
142  // Performance analysis
143  analyzeSpeedResponse(targetRPM, currentRPM, motorCurrent);
144  
145  // MQTT telemetry
146  publishSpeedData(targetRPM, currentRPM, pidOutput, motorCurrent);
147  
148  delay(10); // 100 Hz control loop
149}
150
151ПЕРВАЯ БОЕВАЯ ЗАДАЧА:
152✓ Speed accuracy ±10 RPM
153✓ Settling time < 2 seconds
154✓ Load rejection < 5% speed drop
155✓ Smooth acceleration/deceleration
156✓ Current limiting для motor protection
157✓ Feed-forward compensation

  1🚁 HARDWARE SETUP:
  2• ESP32 Flight Controller
  3• MPU6050 IMU (gyro + accelerometer)  
  4• 4x Brushless ESCs
  5• 4x Motors (НЕ ЛЕТАЕМ! Только стабилизация на стенде)
  6• Altitude sensor (VL53L0X)
  7• GPS module (optional)
  8
  9ПОДКЛЮЧЕНИЕ СТАБИЛИЗАЦИИ:
 10MPU6050:    SDA→GPIO21, SCL→GPIO22
 11ESCs:       PWM→GPIO12,13,14,15
 12Altitude:   SDA→GPIO21, SCL→GPIO22  
 13GPS:        RX→GPIO16, TX→GPIO17
 14
 15#include <MPU6050_tockn.h>
 16#include <ESP32Servo.h>
 17
 18MPU6050 mpu6050(Wire);
 19
 20// Triple PID для 3 осей стабилизации
 21class DroneStabilizationPID {
 22private:
 23  // PID controllers для каждой оси
 24  struct PIDAxis {
 25    double kp, ki, kd;
 26    double integral, lastError;
 27    unsigned long lastTime;
 28  };
 29  
 30  PIDAxis roll, pitch, yaw;
 31  
 32public:
 33  DroneStabilizationPID() {
 34    // Настройки для каждой оси (могут быть разными!)
 35    roll = {2.0, 0.5, 0.1, 0, 0, 0};
 36    pitch = {2.0, 0.5, 0.1, 0, 0, 0};
 37    yaw = {1.5, 0.2, 0.05, 0, 0, 0};
 38  }
 39  
 40  struct ControlOutputs {
 41    double rollOutput, pitchOutput, yawOutput;
 42  };
 43  
 44  ControlOutputs compute(double targetRoll, double currentRoll,
 45                        double targetPitch, double currentPitch,
 46                        double targetYaw, double currentYaw) {
 47    
 48    ControlOutputs outputs;
 49    outputs.rollOutput = computeAxis(roll, targetRoll, currentRoll);
 50    outputs.pitchOutput = computeAxis(pitch, targetPitch, currentPitch);
 51    outputs.yawOutput = computeAxis(yaw, targetYaw, currentYaw);
 52    
 53    return outputs;
 54  }
 55  
 56private:
 57  double computeAxis(PIDAxis& axis, double target, double current) {
 58    unsigned long now = millis();
 59    double dt = (now - axis.lastTime) / 1000.0;
 60    
 61    if (dt <= 0) return 0;
 62    
 63    double error = target - current;
 64    
 65    // P
 66    double P = axis.kp * error;
 67    
 68    // I with windup protection
 69    axis.integral += error * dt;
 70    axis.integral = constrain(axis.integral, -20, 20);
 71    double I = axis.ki * axis.integral;
 72    
 73    // D
 74    double derivative = (error - axis.lastError) / dt;
 75    double D = axis.kd * derivative;
 76    
 77    axis.lastError = error;
 78    axis.lastTime = now;
 79    
 80    return P + I + D;
 81  }
 82};
 83
 84DroneStabilizationPID stabilizer;
 85
 86// Motor mixing для quadcopter
 87void mixerQuadX(double throttle, double roll, double pitch, double yaw, 
 88                int* motor1, int* motor2, int* motor3, int* motor4) {
 89  // X configuration mixer
 90  *motor1 = throttle + roll + pitch - yaw; // Front Right
 91  *motor2 = throttle - roll + pitch + yaw; // Front Left  
 92  *motor3 = throttle - roll - pitch - yaw; // Rear Left
 93  *motor4 = throttle + roll - pitch + yaw; // Rear Right
 94  
 95  // Constrain to valid ESC range (1000-2000 microseconds)
 96  *motor1 = constrain(*motor1, 1000, 2000);
 97  *motor2 = constrain(*motor2, 1000, 2000);
 98  *motor3 = constrain(*motor3, 1000, 2000);
 99  *motor4 = constrain(*motor4, 1000, 2000);
100}
101
102void loop() {
103  // Read IMU data
104  mpu6050.update();
105  double currentRoll = mpu6050.getAngleX();
106  double currentPitch = mpu6050.getAngleY();
107  double currentYaw = mpu6050.getAngleZ();
108  
109  // Target angles (level flight или from remote control)
110  double targetRoll = 0.0;   // Level
111  double targetPitch = 0.0;  // Level
112  double targetYaw = 0.0;    // Hold heading
113  
114  // Compute stabilization outputs
115  auto outputs = stabilizer.compute(targetRoll, currentRoll,
116                                   targetPitch, currentPitch,
117                                   targetYaw, currentYaw);
118  
119  // Base throttle (НЕ ВЗЛЕТАЕМ! Минимальная тяга для тестирования)
120  int baseThrottle = 1100; // Just above motor start threshold
121  
122  // Mix control outputs to individual motors
123  int motor1, motor2, motor3, motor4;
124  mixerQuadX(baseThrottle, outputs.rollOutput, outputs.pitchOutput, outputs.yawOutput,
125             &motor1, &motor2, &motor3, &motor4);
126  
127  // Send to ESCs (ОСТОРОЖНО! Пропеллеры должны быть сняты!)
128  esc1.writeMicroseconds(motor1);
129  esc2.writeMicroseconds(motor2);
130  esc3.writeMicroseconds(motor3);
131  esc4.writeMicroseconds(motor4);
132  
133  // 🆕 Advanced features
134  
135  // Gyro filtering для noise reduction
136  filterGyroData();
137  
138  // Complementary filter для angle estimation
139  complementaryFilter();
140  
141  // Failsafe monitoring
142  if (abs(currentRoll) > 45 || abs(currentPitch) > 45) {
143    emergencyShutdown(); // Cut all motors
144  }
145  
146  // Telemetry
147  publishDroneData(currentRoll, currentPitch, currentYaw, 
148                   motor1, motor2, motor3, motor4);
149  
150  delay(5); // 200 Hz control loop (fast для stability!)
151}
152
153// 🆕 Advanced stabilization features
154void adaptiveGainScheduling() {
155  // Adjust PID gains based on flight conditions
156  double velocity = calculateVelocity();
157  double batteryVoltage = readBatteryVoltage();
158  
159  if (velocity > 5.0) {
160    // High speed: reduce gains для stability
161    stabilizer.adjustGains(0.8);
162  } else if (velocity < 1.0) {
163    // Hovering: increase gains для precision
164    stabilizer.adjustGains(1.2);
165  }
166  
167  // Compensate for battery voltage drop
168  double voltageCompensation = 12.0 / batteryVoltage;
169  stabilizer.adjustThrottleCompensation(voltageCompensation);
170}
171
172ПЕРВАЯ БОЕВАЯ ЗАДАЧА:
173✓ Stable hovering (±2 degrees)
174✓ Quick recovery from disturbances
175✓ Smooth response to commands
176✓ No oscillations
177✓ Battery voltage compensation
178✓ EMERGENCY failsafe working

  1// 🏭 MASTER CONTROL SYSTEM
  2#include <WiFi.h>
  3#include <PubSubClient.h>
  4#include <ArduinoJson.h>
  5
  6// Global system state
  7struct SystemState {
  8  // Temperature control
  9  double currentTemp, targetTemp, heaterOutput;
 10  
 11  // Position control  
 12  double currentPosition, targetPosition, servoOutput;
 13  
 14  // Speed control
 15  double currentRPM, targetRPM, motorOutput;
 16  
 17  // Drone stabilization
 18  double roll, pitch, yaw;
 19  
 20  // System health
 21  bool allSystemsOperational;
 22  double overallEfficiency;
 23};
 24
 25SystemState globalState;
 26
 27// 🎯 COORDINATED CONTROL SEQUENCES
 28void executeManufacturingSequence() {
 29  Serial.println("Starting coordinated manufacturing sequence...");
 30  
 31  // Phase 1: Temperature preheating
 32  publishMQTT("systems/temperature/setpoint", "60.0");
 33  waitForCondition([]() { return abs(globalState.currentTemp - 60.0) < 1.0; }, 30000);
 34  
 35  // Phase 2: Position robot arm for pickup
 36  publishMQTT("systems/position/target", "45.0");
 37  waitForCondition([]() { return abs(globalState.currentPosition - 45.0) < 2.0; }, 5000);
 38  
 39  // Phase 3: Start conveyor at precise speed
 40  publishMQTT("systems/speed/target", "100.0");
 41  waitForCondition([]() { return abs(globalState.currentRPM - 100.0) < 5.0; }, 3000);
 42  
 43  // Phase 4: Coordinate all systems
 44  coordinated_operation();
 45  
 46  Serial.println("Manufacturing sequence completed!");
 47}
 48
 49void coordinated_operation() {
 50  unsigned long startTime = millis();
 51  
 52  while (millis() - startTime < 60000) { // 1 minute operation
 53    
 54    // 🔄 Dynamic coordination based on system state
 55    
 56    // If temperature too high, slow down conveyor
 57    if (globalState.currentTemp > 65.0) {
 58      double reducedSpeed = globalState.targetRPM * 0.8;
 59      publishMQTT("systems/speed/target", String(reducedSpeed));
 60    }
 61    
 62    // If conveyor jammed (low RPM), adjust robot timing
 63    if (globalState.currentRPM < globalState.targetRPM * 0.9) {
 64      delay(100); // Wait for conveyor to catch up
 65    }
 66    
 67    // Adaptive position control based on speed
 68    double speedCompensatedPosition = calculateDynamicPosition(globalState.currentRPM);
 69    publishMQTT("systems/position/target", String(speedCompensatedPosition));
 70    
 71    // 🆕 Machine learning optimization
 72    optimizeSystemParameters();
 73    
 74    delay(100); // 10 Hz coordination loop
 75  }
 76}
 77
 78// 🤖 MACHINE LEARNING PID OPTIMIZATION
 79class MLPIDOptimizer {
 80private:
 81  struct PerformanceMetrics {
 82    double settlingTime;
 83    double overshoot;
 84    double steadyStateError;
 85    double energyConsumption;
 86    double score; // Combined performance score
 87  };
 88  
 89  std::vector<PerformanceMetrics> historicalPerformance;
 90  
 91public:
 92  void recordPerformance(double Kp, double Ki, double Kd, 
 93                        double settlingTime, double overshoot, 
 94                        double steadyStateError, double power) {
 95    
 96    PerformanceMetrics metrics;
 97    metrics.settlingTime = settlingTime;
 98    metrics.overshoot = overshoot;
 99    metrics.steadyStateError = steadyStateError;
100    metrics.energyConsumption = power;
101    
102    // Calculate composite score (lower is better)
103    metrics.score = settlingTime * 0.3 + overshoot * 0.3 + 
104                   steadyStateError * 100 * 0.2 + power * 0.2;
105    
106    historicalPerformance.push_back(metrics);
107    
108    // Keep only recent data
109    if (historicalPerformance.size() > 100) {
110      historicalPerformance.erase(historicalPerformance.begin());
111    }
112  }
113  
114  struct PIDGains {
115    double Kp, Ki, Kd;
116  };
117  
118  PIDGains suggestOptimalGains() {
119    // Find best performing parameter set
120    double bestScore = 999999;
121    int bestIndex = -1;
122    
123    for (int i = 0; i < historicalPerformance.size(); i++) {
124      if (historicalPerformance[i].score < bestScore) {
125        bestScore = historicalPerformance[i].score;
126        bestIndex = i;
127      }
128    }
129    
130    if (bestIndex >= 0) {
131      // Return gains that produced best performance
132      return getBestGains(bestIndex);
133    }
134    
135    // Default gains if no data
136    return {1.0, 0.1, 0.01};
137  }
138  
139  // 🧠 Simple neural network для prediction
140  PIDGains neuralNetworkOptimization(double currentError, double systemLoad) {
141    // Simplified neural network logic
142    // In reality, this would be a trained network
143    
144    double normalizedError = constrain(currentError / 100.0, -1, 1);
145    double normalizedLoad = constrain(systemLoad / 1000.0, 0, 1);
146    
147    // Simple weighted combination (trained weights)
148    double Kp = 1.0 + normalizedError * 0.5 + normalizedLoad * 0.3;
149    double Ki = 0.1 + abs(normalizedError) * 0.05;
150    double Kd = 0.01 + normalizedLoad * 0.02;
151    
152    return {Kp, Ki, Kd};
153  }
154};
155
156MLPIDOptimizer optimizer;
157
158// 🎯 SYSTEM HEALTH MONITORING
159void monitorSystemHealth() {
160  // Check all PID controllers performance
161  double tempPerformance = evaluateTemperatureControl();
162  double positionPerformance = evaluatePositionControl();
163  double speedPerformance = evaluateSpeedControl();
164  
165  globalState.overallEfficiency = (tempPerformance + positionPerformance + speedPerformance) / 3.0;
166  
167  // Automatic retuning if performance degrades
168  if (globalState.overallEfficiency < 0.8) {
169    Serial.println("Performance degradation detected. Initiating auto-retune...");
170    autoRetuneAllSystems();
171  }
172  
173  // Predictive maintenance alerts
174  if (globalState.overallEfficiency < 0.6) {
175    publishAlert("CRITICAL: System performance critically low. Maintenance required!");
176  }
177  
178  // Publish health metrics
179  publishSystemHealth();
180}
181
182// 🔮 PREDICTIVE ANALYTICS
183void predictiveAnalytics() {
184  // Analyze trends in system performance
185  std::vector<double> performanceHistory = getPerformanceHistory(100); // Last 100 samples
186  
187  // Simple linear regression для trend detection
188  double trend = calculateTrend(performanceHistory);
189  
190  if (trend < -0.01) { // Performance declining
191    double timeToFailure = estimateTimeToFailure(trend);
192    
193    if (timeToFailure < 24.0) { // Hours
194      publishAlert("PREDICTIVE: System failure predicted in " + String(timeToFailure) + " hours");
195    }
196  }
197  
198  // Suggest optimization actions
199  suggestOptimizations();
200}

1ЗАДАЧА: Температурный step response competition
2✓ Target: 0°C → 50°C → 25°C → 75°C sequence
3✓ Scoring: Speed + Accuracy + Energy efficiency
4✓ Penalties: Overshoot >10%, Steady-state error >1°C
5✓ BONUS: Fastest settling time без overshoot (+50 points)
6✓ MQTT real-time performance metrics