コード例 #1
0
        /// <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);
        }