Вот кусок кода для регулятора:
- Код: Выделить всё • Развернуть
//Функция масштабирования значений
private double ScaleValue(double value, double valuemin,
double valuemax, double scalemin, double scalemax)
{
double vPerc = (value - valuemin) / (valuemax - valuemin);
double bigSpan = vPerc * (scalemax - scalemin);
double retVal = scalemin + bigSpan;
return retVal;
}
//Определение мин-макс границ
private double Clamp(double value, double min, double max)
{
if (value > max)
return max;
if (value < min)
return min;
return value;
}
//Собственно регулятор
private void Compute()
{
//Значение компаса
double pv = GlobalData.Azimuth;
//Задание автопилота
double sp = GlobalData.Auto_Azimuth;
pv = Clamp(pv, pvMin, pvMax);
//Масштабируем текущее состояние
pv = ScaleValue(pv, pvMin, pvMax, -1.0f, 1.0f);
//..И задание
sp = Clamp(sp, pvMin, pvMax);
sp = ScaleValue(sp, pvMin, pvMax, -1.0f, 1.0f);
//Теперь значение ошибки в процентах
double err = sp - pv;
//Начальные значения для временных переменных коэффициентов, считаем пропорциональную часть
double pTerm = err * kp;
double iTerm = 0.0f;
double dTerm = 0.0f;
double partialSum = 0.0f;
//Запоминаем текущее время
Date nowTime = new Date();
if (lastUpdate != null)
{
double dT = ((double)nowTime.getTime() - (double)lastUpdate.getTime())/1000;
//Compute the integral if we have to...
if (pv >= pvMin && pv <= pvMax)
{
//Если нужно, считаем интегральную часть
if (ki>0)
{
partialSum = errSum + dT * err;
iTerm = ki * partialSum;
}
}
//..и дифференциальную
if (kd>0)
{
if (dT != 0.0f)
dTerm = kd * (pv - lastPV) / dT;
}
}
//обновляем время
lastUpdate = nowTime;
errSum = partialSum;
lastPV = pv;
//Расчет результата
double outReal = pTerm + iTerm + dTerm;
outReal = Clamp(outReal, -1.0f, 1.0f);
outReal = ScaleValue(outReal, -1.0f, 1.0f, outMin, outMax);
//Выводим результат
GlobalData.Auto_PID = outReal;
}
//Вывод на двигатели, Auto_MedPowe- задаваемое значение, с какой средней мощностью должны работать двигатели
double temp;
//Левый двигатель
temp=Auto_MedPower+GlobalData.Auto_PID;
GlobalData.Engine1=(byte)temp;
//правый двигатель
temp=Auto_MedPower-GlobalData.Auto_PID;
GlobalData.Engine2=(byte)temp;
