pidloop.c

稀疏矩阵、链表、图、队列、二叉树、多叉树、排序、遗传算法等的实现

  char              *pequal;
  PARAM_PARSE       *pp;
  double            value;
  
  /* allow interactive input                          */
  do
  {
    show_params = 0;    /* don't show the menu        */
    ch = getchar();
    if (ch == EOF)
    {
      break;
    }
    else
    {
      ch = toupper((unsigned char)ch);
    }
    
    if (isupper(ch))
    {
      /* see if the letter represents a parameter     */
      pp = bsearch(&ch, parse,
                   sizeof parse / sizeof *parse,
                   sizeof *parse, Compare);

      if (NULL != pp) /* found a match                */
      {
        /* allow for the complete entry on the line   */
        if (NULL != fgets(inbuff, sizeof inbuff, stdin))
        {
          /* check for equal sign in rest of line     */
          if (NULL != (pequal = strchr(inbuff, '=')))
          {
            /* yes, get the value                     */
            ParseValue(pp, pequal + 1, pp->destination);
            show_params = 1;  /* show all values      */
          }
          else  /* NULL == (pequal = strchr(...))     */
          {
            /* display a prompt for the value         */
            /* get current value for display          */
            value = *(pp->destination);
        
            if (pp->force_to_int)
            {
              fprintf(stderr, "\nEnter %s current value "
                      "%0.0f range %0.0f to %0.0f ",
                      pp->param_name, value,
                      pp->min_val,
                      pp->max_val);
            }
            else
            {
              fprintf(stderr, "\nEnter %s current value "
                      "%0.4f range %0.4f to %0.4f ",
                      pp->param_name, value, pp->min_val,
                      pp->max_val);
            }
            fflush(stderr);

            if (NULL != fgets(inbuff,
                sizeof inbuff, stdin))
            {
              ParseValue(pp, inbuff, pp->destination);
              show_params = 1;  /* show all values    */
            }
          }     /* else NULL == (pequal = strchr(...))*/
        }     /* if (NULL != fgets(...)               */
      }     /* if (NULL != pp)                        */
    }     /* isupper(ch)                              */
    else if ('?' == ch)
    {
      /* user requesting menu and current values      */
      if ((ch = ClearStdin()) == EOF)
      {
        break;
      }
      show_params = 1;
    }

    /* if a new cycle count was just sent, return a   */
    /* continue status and go back to automatic       */
    if (0 != params.cycles)
    {
      ch = '\n';
    }
    else if (show_params)
    {
      ShowParams();   /* display values and menu      */
      ch = 0;         /* force another pass           */
    }

  } while (ch != '\n' && ch != EOF && ch != 'Q' && ch != 'q');

  return (ch == '\n');  
}
  
/*********************************************************
 *
 * Function:    GetCurrentPV
 *
 * Parameters:  double output, the output value calculated
 *              by the last call to ComputerPID()
 *
 * Returns:     the new value of the process variable (PV)
 *
 * Description: simulates the response of the system being
 *              controlled, taking the value of the lag
 *              member of the PID_PARAMS structure into
 *              account
 *
 * Algorithm:   if the output value remains the same for
 *              a great length of time the process variable
 *              will eventually reach and remain at a value
 *              determined entirely by the transfer ratio
 *              (params.trans) times the output value
 *
 *              the more inertia the controlled system has,
 *              that is the higher the lag member, the longer
 *              this takes
 *
 *              if the PV on the last iteration is not the
 *              same as the final PV value for the new
 *              output, the new PV will be somewhere in
 *              between the two
 *
 *              if the lag value is near 1 (high inertia),
 *              the PV will only change a small part of the
 *              way to the value indicated by the new
 *              output
 *
 *              if the lag value is near 0 (low inertia),
 *              the PV will change most of the way to the
 *              value indicated by the new output
 *
 *              the extremes are a lag of 1, in which case
 *              the system is a perpetual motion machine and
 *              the PV will never change no matter what the
 *              output does, and a lag of 0, in which
 *              case the PV responds fully to a change in
 *              output completely by the next pass through
 *              the loop
 *
 ********************************************************/
static double
GetCurrentPV(double output)
{
  static double     build_up                = 0.0;
         double     value;

  build_up += output;
  value = build_up * (1 - params.lag) * params.trans;
  build_up *= params.lag;
  return value;
}

/*********************************************************
 *
 * Function:    ComputePID
 *
 * Parameters:  this contains the actual PID calculation
 *              algorithm, including optional accelleration,
 *              velocity, and friction feed forwards, and
 *              a slew rate limitation
 *
 * Returns:     a double containg the calculated new
 *              output value
 *
 * Description: main control algorithm
 *
 ********************************************************/
static double
ComputePID(double PV)
{
  /* the three static variables are required to retain  */
  /* information between passes through the loop        */
  static double     integral                = 0.0;
  static double     last_error              = 0.0;
  static double     last_output             = 0.0;

         double     this_error;
         double     this_output;
         double     accel;
         double     deriv;
         double     friction;

  /* the desired PV for this iteration is the value     */
  /* calculated as next_target during the last loop     */
  this_target = next_target;
  
  /* test for acceleration, compute new target PV for   */
  /* the next pass through the loop                     */
  if (params.accel > 0 && this_target != params.setpt)
  {
    if (this_target < params.setpt)
    {    
      next_target += params.accel;
      if (next_target > params.setpt)
      {
        next_target = params.setpt;
      }
    }
    else /* params.target > params.setpoint */
    {    
      next_target -= params.accel;
      if (next_target < params.setpt)
      {
        next_target = params.setpt;
      }
    }
  }
  else
  {
    next_target = params.setpt;
  }
  
  /* acceleration is the difference between the PV      */
  /* target on this pass and the next pass through the  */
  /* loop                                               */
  accel = next_target - this_target;

  /* the error for the current pass is the difference   */
  /* between the current target and the current PV      */
  this_error = this_target - PV;

  /* the derivative is the difference between the error */
  /* for the current pass and the previous pass         */
  deriv = this_error - last_error;

  /* a very simple determination of whether there is    */
  /* special friction to be overcome on the next pass,  */
  /* if the current PV is 0 and the target for the next */
  /* pass is not 0, stiction could be a problem         */
  friction = (PV == 0.0 && next_target != 0.0);

  /* the new error is added to the integral             */
  integral += this_target - PV;

  /* the square of the error is accumulated in          */
  /* rms_error, for reporting at the end of the program */
  /* it has no part in the PID loop calculations        */
  rms_error += (this_error * this_error);

  /* now that all of the variable terms have been       */
  /* computed they can be multiplied by the appropriate */
  /* coefficients and the resulting products summed     */
  this_output = params.p_gain * this_error
              + params.i_gain * integral
              + params.d_gain * deriv
              + params.acc_ff * accel
              + params.vel_ff * next_target
              + params.fri_ff * friction
              + params.bias;

  last_error   = this_error;

  /* check for slew rate limiting on the output change  */
  if (0 != params.slew)
  {
    if (this_output - last_output > params.slew)
    {
      this_output = last_output + params.slew;
    }
    else if (last_output - this_output > params.slew)
    {
      this_output = last_output - params.slew;
    }
  }
  
  /* now check the output value for absolute limits     */
  if (this_output < params.min)
  {
    this_output = params.min;
  }
  else if (this_output > params.max)
  {
    this_output = params.max;
  }
  
  /* store the new output value to be used as the old   */
  /* output value on the next loop pass                 */
  return last_output = this_output;
}

/*********************************************************
 *
 * Function:    main
 *
 * Parameters:  int argc, number of command line parameters,
 *              including the name of the executable, if
 *              the platform provides it
 *
 *              char **argv, array of pointers to '\0'
 *              terminated strings containing the
 *              executable name, if available, and the
 *              command line arguments
 *
 * Returns:     integer value of 0
 *
 * Description: entry and main driver for the PID
 *              simulation
 *
 ********************************************************/
int main(int argc, char **argv)
{
  double            current_output          = 0.0;
  double            old_output              = 0.0;
  unsigned long     repetitions             = 0;
  unsigned long     hold_time               = 0;
  int               verbose                 = 0;
  double            PV;
  char              results [100];
  char              *args;
  int               current_arg;
  
  /* this eliminates a warning on some compilers,     */
  /* comment it out if you don't need it              */
  argc = argc;
  
  /* check command line arguments, can accept one     */
  /* -v or -V and one representing a file name with   */
  /* paremeter and event settings                     */
  for (current_arg = 1;
       NULL != (args = argv[current_arg]);
       ++current_arg)
  {
    if (*args == '-')
    {
      if (args[1] == 'v' || args[1] == 'V')
      {
        verbose = 1;
      }
    }
    else
    {
      ParseParams(args);
    }
  }
  
  params.cycles = floor(params.cycles);

  ShowParams();

  /* before starting the loop, check for the case of no */
  /* acceleration and preset the this_target and        */
  /* next_target variables                              */
  if (0.0 == params.accel)
  {
    this_target = next_target = params.setpt;
  }
 
  /* loop until a quit event is returned by CheckEvents */
  while (0 == CheckEvents(repetitions))
  {
    /* only call interactive function Continue() if     */
    /* there is no preset continuous cycle count, exit  */
    /* if Continue() receives quit command or EOF       */
    if (0 == params.cycles && 0 == Continue())
    {
      break;
    }

    PV = GetCurrentPV(current_output);
    current_output = ComputePID(PV);
    if (hold_time > 0)
    {
      --hold_time;
      current_output = old_output;
    }
    else if (current_output != old_output)
    {
      hold_time = (unsigned long)floor(params.hold);
      old_output = current_output;
    }

    sprintf(results,
          "%5lu: SP %7.2f PV %7.2f Out %7.2f ",
           ++repetitions, this_target, PV,
           current_output);
    fputs(results, stdout);
    if (verbose)
    {
      fputs(results, stderr);
      putc('\n', stderr);
    }
    if (params.cycles)
    {
      params.cycles = floor(params.cycles) - 1;
      putc('\n', stdout);
    }
  }

  if (repetitions)
  {
    rms_error /= repetitions;
  }
  else
  {
    rms_error = 0;
  }
  
  fprintf(stderr, "\n%lu repetitions, RMS error %f\n",
          repetitions, sqrt(rms_error));
  
  return 0;
}