/// <summary>
/// computes PID output
/// </summary>
/// <param name="setpointMks"></param>
/// <param name="measuredValueMks"></param>
/// <param name="millis">current time value</param>
/// <returns></returns>
private double ComputeServoInputWithPID(double servoInputMks, double setpointMks, double measuredValueMks, ulong millis)
{
// we come here every 50ms, millis is a very large number increasing by 50 every time
//servoInputMks = setpointMks;
// don't allow PID inputs exceed platform limitations:
setpointMks = GeneralMath.constrain(setpointMks, (double)panKinectMksMin, (double)panKinectMksMax);
measuredValueMks = GeneralMath.constrain(measuredValueMks, (double)panKinectMksMin, (double)panKinectMksMax);
pidA.mySetpoint = setpointMks; // around 1500. Where we want to be.
pidA.myInput = measuredValueMks; // around 1500. Where we currently are.
pidA.Compute(millis);
// pidA operates on error=mySetpoint - myInput
// produces pidA.myOutput - a number around 0, limited by pidA.SetOutputLimits()
//Tracer.Trace("pidA: setpointMks=" + setpointMks + " measuredValueMks=" + measuredValueMks + " millis=" + millis + " pidA.myOutput=" + pidA.myOutput);
// don't allow PID output exceed platform limitations:
servoInputMks = GeneralMath.constrain(servoInputMks + pidA.myOutput, (double)panKinectMksMin, (double)panKinectMksMax);
return(servoInputMks);
}