215 lines
7.7 KiB
C++
215 lines
7.7 KiB
C++
#include "History.h"
|
|
#include "Config.h"
|
|
|
|
extern class Preferences preferences;
|
|
CHistory history;
|
|
|
|
CHistory::CHistory()
|
|
: head(0), tail(0), count(0), lastTemp(0), lastHumid(0)
|
|
{
|
|
Kd_Humidity = 3.6; // Load Kd for humidity
|
|
Kd_Humidity = 1.8;
|
|
LR_Humidity = 0.25;
|
|
|
|
Kp_Temp1 = 3.6;
|
|
Kd_Temp1 = 1.8;
|
|
LR_Temp1 = 0.25;
|
|
|
|
head = 0;
|
|
tail = 0;
|
|
count = 0;
|
|
|
|
lastTemp = 0;
|
|
lastHumid = 0;
|
|
for (int i = 0; i < RING_SIZE; i++)
|
|
ring[i] = {{0}};
|
|
}
|
|
|
|
void CHistory::init(int16_t _lastTemp, int16_t _lastHumid) {
|
|
lastTemp = _lastTemp;
|
|
lastHumid = _lastHumid;
|
|
}
|
|
|
|
void CHistory::loadPID() {
|
|
Kp_Humidity = config.Kp_Humidity;
|
|
Kd_Humidity = config.Kd_Humidity;
|
|
LR_Humidity = config.LR_Humidity;
|
|
|
|
Kp_Temp1 = config.Kp_Temp1;
|
|
Kd_Temp1 = config.Kd_Temp1;
|
|
LR_Temp1 = config.LR_Temp1;
|
|
|
|
Kp_Temp2 = config.Kp_Temp2;
|
|
Kd_Temp2 = config.Kd_Temp2;
|
|
LR_Temp2 = config.LR_Temp2;
|
|
|
|
Kp_Temp3 = config.Kp_Temp3;
|
|
Kd_Temp3 = config.Kd_Temp3;
|
|
LR_Temp3 = config.LR_Temp3;
|
|
|
|
}
|
|
|
|
void CHistory::savePID() {
|
|
config.Kp_Humidity = Kp_Humidity;
|
|
config.Kd_Humidity = Kd_Humidity;
|
|
config.LR_Humidity = LR_Humidity;
|
|
|
|
config.Kp_Temp1 = Kp_Temp1;
|
|
config.Kd_Temp1 = Kd_Temp1;
|
|
config.LR_Temp1 = LR_Temp1;
|
|
|
|
config.Kp_Temp2 = Kp_Temp2;
|
|
config.Kd_Temp2 = Kd_Temp2;
|
|
config.LR_Temp2 = LR_Temp2;
|
|
|
|
config.Kp_Temp3 = Kp_Temp3;
|
|
config.Kd_Temp3 = Kd_Temp3;
|
|
config.LR_Temp3 = LR_Temp3;
|
|
}
|
|
|
|
// Method to add status data to the history buffer
|
|
MY_IRAM_ATTR short CHistory::add(STATUS_TYPE &status) {
|
|
|
|
ring[head] = status;
|
|
|
|
if (++head >= RING_SIZE) head = 0; // Mask with 0xFF for wrap-around
|
|
if (count < RING_SIZE) {
|
|
count++;
|
|
} else {
|
|
if (++tail > RING_SIZE) tail = 0; // Advance tail if buffer is full
|
|
}
|
|
return count;
|
|
}
|
|
|
|
// Main PD Control method for heater duty calculation
|
|
MY_IRAM_ATTR int16_t CHistory::calculateDutyForTemp1(int16_t setpoint, int16_t curTemp, int16_t lastDuty) {
|
|
// Calculate Proportional term
|
|
float currentError = setpoint - curTemp;
|
|
float P = Kp_Temp1 * currentError;
|
|
|
|
// Calculate Derivative term based on temperature slope
|
|
float timeInterval = 10.0; // Assuming a 5-second interval between updates; adjust as needed
|
|
float slope = (curTemp - lastTemp) / timeInterval;
|
|
float D = Kd_Temp1 * (-slope); // Negative sign to counteract the rising trend
|
|
|
|
// Limit magnitute of change
|
|
int16_t diff = roundf(P + D);
|
|
if (diff > 500) diff = 500;
|
|
else if (diff < -500) diff = -500;
|
|
|
|
// Calculate new heater duty cycle
|
|
int16_t newDuty = lastDuty + diff;
|
|
|
|
// Constrain the duty cycle within the range of 0 to 10000
|
|
if (newDuty < 0) newDuty = 0;
|
|
else if (newDuty > 10000) newDuty = 10000;
|
|
|
|
// Adjust Kp and Kd using gradient descent based on the current and previous errors
|
|
adjustGainsUsingGradientDescent(Kp_Temp1, Kd_Temp1, currentError, setpoint, lastTemp, 100.0f);
|
|
if (Kp_Temp1 < 0.5f * config.Kp_Temp1) Kp_Temp1 = 0.5f * config.Kp_Temp1;
|
|
else if (Kp_Temp1 > 2.0f * config.Kp_Temp1) Kp_Temp1 = 2.0f * config.Kp_Temp1;
|
|
if (Kd_Temp1 < 0.5f * config.Kd_Temp1) Kd_Temp1 = 0.5f * config.Kd_Temp1;
|
|
else if (Kd_Temp1 > 2.0f * config.Kd_Temp1) Kd_Temp1 = 2.0f * config.Kd_Temp1;
|
|
|
|
lastTemp = curTemp;
|
|
return newDuty;
|
|
}
|
|
|
|
|
|
MY_IRAM_ATTR int16_t CHistory::calculateDutyForTemp2(int16_t setpoint, int16_t curTemp, int16_t lastDuty) {
|
|
// Calculate Proportional term
|
|
float currentError = setpoint - curTemp;
|
|
float P = Kp_Temp2 * currentError;
|
|
|
|
// Calculate Derivative term based on temperature slope
|
|
float timeInterval = 10.0; // Assuming a 5-second interval between updates; adjust as needed
|
|
float slope = (curTemp - lastTemp) / timeInterval;
|
|
float D = Kd_Temp2 * (-slope); // Negative sign to counteract the rising trend
|
|
|
|
// Limit magnitute of change
|
|
int16_t diff = roundf(P + D);
|
|
if (diff > 500) diff = 500;
|
|
else if (diff < -500) diff = -500;
|
|
|
|
// Calculate new heater duty cycle
|
|
int16_t newDuty = lastDuty + diff;
|
|
|
|
// Constrain the duty cycle within the range of 0 to 10000
|
|
if (newDuty < 0) newDuty = 0;
|
|
else if (newDuty > 10000) newDuty = 10000;
|
|
|
|
// Adjust Kp and Kd using gradient descent based on the current and previous errors
|
|
adjustGainsUsingGradientDescent(Kp_Temp2, Kd_Temp2, currentError, setpoint, lastTemp, 100.0f);
|
|
if (Kp_Temp2 < 0.5f * config.Kp_Temp2) Kp_Temp2 = 0.5f * config.Kp_Temp2;
|
|
else if (Kp_Temp2 > 2.0f * config.Kp_Temp2) Kp_Temp2 = 2.0f * config.Kp_Temp2;
|
|
if (Kd_Temp2 < 0.5f * config.Kd_Temp2) Kd_Temp2 = 0.5f * config.Kd_Temp2;
|
|
else if (Kd_Temp2 > 2.0f * config.Kd_Temp2) Kd_Temp2 = 2.0f * config.Kd_Temp2;
|
|
|
|
lastTemp = curTemp;
|
|
return newDuty;
|
|
}
|
|
|
|
|
|
MY_IRAM_ATTR int16_t CHistory::calculateMistDuty(uint16_t setpoint, uint16_t curHumid, int16_t lastDuty) {
|
|
// Calculate Proportional (P) term based on the error (difference between setpoint and current humidity)
|
|
float currentError = setpoint - curHumid;
|
|
float P = Kp_Humidity * currentError;
|
|
|
|
// Calculate Derivative (D) term based on humidity slope
|
|
float timeInterval = 10.0; // Assuming a 10-second interval between updates; adjust as needed
|
|
float slope = (curHumid - lastHumid) / timeInterval;
|
|
float D = Kd_Humidity * (-slope); // Negative sign to counteract the rising or falling trend
|
|
|
|
// Limit magnitute of change
|
|
int16_t diff = roundf(P + D);
|
|
if (diff > 500) diff = 500;
|
|
else if (diff < -500) diff = -500;
|
|
|
|
// Calculate the new mist duty cycle
|
|
int16_t newDuty = lastDuty + diff;
|
|
|
|
// Constrain the duty cycle within the allowable range (0 to 511 for 50% duty cycle)
|
|
if (newDuty < 0) newDuty = 0;
|
|
else if (newDuty > 10000) newDuty = 10000;
|
|
|
|
// Adjust Kp and Kd using gradient descent based on current and previous errors
|
|
adjustGainsUsingGradientDescent(Kp_Humidity, Kd_Humidity, currentError, setpoint, lastHumid, 300.0f);
|
|
if (Kp_Humidity < 0.5f * config.Kp_Humidity) Kp_Humidity = 0.5f * config.Kp_Humidity;
|
|
else if (Kp_Humidity > 2.0f * config.Kp_Humidity) Kp_Humidity = 2.0f * config.Kp_Humidity;
|
|
if (Kd_Humidity < 0.5f * config.Kd_Humidity) Kd_Humidity = 0.5f * config.Kd_Humidity;
|
|
else if (Kd_Humidity > 2.0f * config.Kd_Humidity) Kd_Humidity = 2.0f * config.Kd_Humidity;
|
|
|
|
lastHumid = curHumid;
|
|
return newDuty;
|
|
}
|
|
|
|
// Method to adjust Kp and Kd using gradient descent based on current and previous errors
|
|
MY_IRAM_ATTR void CHistory::adjustGainsUsingGradientDescent(float &Kp, float &Kd, float currentError, uint16_t setpoint, float prevTemperature, float MAX_EXPECTED_ERROR) {
|
|
// Normalize the error (assuming a max expected error for normalization)
|
|
float normalizedError = currentError / MAX_EXPECTED_ERROR;
|
|
|
|
// Calculate change in temperature as the slope
|
|
float temperatureSlope = (setpoint - prevTemperature);
|
|
|
|
// Define limits for adjusting Kp and Kd
|
|
const float proportionalLimitFactorKp = 0.25f;
|
|
const float proportionalLimitFactorKd = 0.25f;
|
|
|
|
// Scale the gradients based on normalized error and temperature slope
|
|
float limitedProportionalGradient = proportionalLimitFactorKp * Kp;
|
|
float limitedDerivativeGradient = proportionalLimitFactorKd * Kd;
|
|
|
|
// Update Kp and Kd using their respective gradients, ensuring adjustments are within limits
|
|
Kp -= LR_Temp1 * fmin(fabs(normalizedError), limitedProportionalGradient) * (normalizedError < 0 ? 1 : -1);
|
|
Kd -= LR_Temp1 * fmin(fabs(temperatureSlope), limitedDerivativeGradient) * (temperatureSlope < 0 ? 1 : -1);
|
|
|
|
// Constrain Kp and Kd to avoid going below minimum threshold values
|
|
const float MIN_KP = 0.01f;
|
|
const float MIN_KD = 0.01f;
|
|
Kp = max(Kp, MIN_KP);
|
|
Kd = max(Kd, MIN_KD);
|
|
}
|
|
|
|
|
|
|