/// <summary> /// Calculates the output given the target value and actual value. /// </summary> /// <param name="setpoint">Target</param> /// <param name="processVariable">Actual</param> /// <returns></returns> public double Calculate(double setpoint, double processVariable) { //samplingPeriod = DateTime.Now.Subtract(dateTime).TotalSeconds + 0.08; samplingPeriod = 0.1; if (samplingPeriod > 0) { precisionCoefficient = samplingPeriod * precisionModifier; Tuple <double, double> td = TrackingDifferentiator.Track(setpoint, samplingPeriod); //double input Tuple <double, double, double> eso = ExtendedStateObserver.ObserveState(samplingPeriod, output, plantCoefficient, processVariable); //double u, double y, double b0 output = NonlinearCombiner.Combine(td, plantCoefficient, eso, precisionCoefficient); dateTime = DateTime.Now; } return(MathFunctions.Constrain(output, -maxOutput, maxOutput)); }
/// <summary> /// Calculates the output given the target value and actual value. /// </summary> /// <param name="setpoint">Target</param> /// <param name="processVariable">Actual</param> /// <returns></returns> public double Calculate(double setpoint, double processVariable) { samplingPeriod = DateTime.Now.Subtract(dateTime).TotalSeconds; if (samplingPeriod > 0) { precisionCoefficient = samplingPeriod * precisionModifier; double pdValue = pid.Calculate(setpoint, processVariable, samplingPeriod); Tuple <double, double> pd = new Tuple <double, double>(pdValue, previousPD); Tuple <double, double, double> eso = ExtendedStateObserver.ObserveState(samplingPeriod, output, plantCoefficient, processVariable);//double u, double y, double b0 output = NonlinearCombiner.Combine(pd, plantCoefficient, eso, precisionCoefficient); previousPD = pdValue; dateTime = DateTime.Now; } return(MathFunctions.Constrain(output, -maxOutput, maxOutput)); }