roboforum.ru

//Функция масштабирования значений
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;