public static IObservable <AngularVelocity3> Manipulate(
this IObservable <AngularVelocity3> processVariable, IObservable <AngularVelocity3> setpoint,
double proportionalGainX, double integralGainX, double derivativeGainX, double maxErrorCumulativeX,
double proportionalGainY, double integralGainY, double derivativeGainY, double maxErrorCumulativeY,
double proportionalGainZ, double integralGainZ, double derivativeGainZ, double maxErrorCumulativeZ)
{
var controller = new AngularVelocityPidController(
proportionalGainX, integralGainX, derivativeGainX, maxErrorCumulativeX,
proportionalGainY, integralGainY, derivativeGainY, maxErrorCumulativeY,
proportionalGainZ, integralGainZ, derivativeGainZ, maxErrorCumulativeZ);
return(Manipulate(processVariable, setpoint, controller));
}
public static IObservable <AngularVelocity3> Manipulate(
this IObservable <AngularVelocity3> processVariable,
IObservable <AngularVelocity3> setpoint,
AngularVelocityPidController controller)
{
return(processVariable
.CombineLatest(
setpoint,
(pv, sp) =>
new { ProcessVariable = pv, Setpoint = sp })
.TimeInterval()
.Select(t =>
{
double dt = t.Interval.TotalSeconds;
return controller.Manipulate(t.Value.ProcessVariable, t.Value.Setpoint, dt);
}));
}
