pidloop.c

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

/*********************************************************
 *
 *  pidloop.c - contains code to simulate PID control
 *              loops
 *
 *  Copyright (C) 2000  Jack Klein
 *                      Macmillan Computer Publishing
 *
 *  This program is free software; you can redistribute it
 *  and/or modify it under the terms of the GNU General
 *  Public License as published by the Free Software
 *  Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will
 *  be useful, but WITHOUT ANY WARRANTY; without even the
 *  implied warranty of MERCHANTABILITY or FITNESS FOR A
 *  PARTICULAR PURPOSE.  See the GNU General Public License
 *  for more details.
 *
 *  You should have received a copy of the GNU General
 *  Public License along with this program; if not, write
 *  to the Free Software Foundation, Inc., 675 Mass Ave,
 *  Cambridge, MA 02139, USA.
 *
 *  Jack Klein may be contacted by email at:
 *     [e-mail]
 *
 ********************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <ctype.h>
#include <math.h>

/*********************************************************
 *
 * Data Type:   PID_PARAMS structure
 *
 * Contents:    all values used by the PID simulation,
 *              plus a cycle count
 *
 ********************************************************/
typedef struct
{
  double p_gain;      /* 'P' proportional gain          */
  double i_gain;      /* 'I' integral gain              */
  double d_gain;      /* 'D' derivative gain            */
  double acc_ff;      /* 'A' acceleration feed forward  */
  double fri_ff;      /* 'F' friction feed forward      */
  double vel_ff;      /* 'V' velocity feed forward      */
  double hold;        /* 'H' output Hold                */
  double bias;        /* 'B' bias                       */
  double accel;       /* 'R' acceleration rate          */
  double setpt;       /* 'S' set point                  */
  double trans;       /* 'T' transfer ratio             */
  double lag;         /* 'L' lag in output change       */
  double min;         /* 'N' minimum output value       */
  double max;         /* 'M' maximum output value       */
  double cycles;      /* 'Y' repeat cycle count         */
  double slew;        /* 'W' maximum slew rate          */
} PID_PARAMS;

static PID_PARAMS params =
{
   0.00,              /* 'P' proportional gain          */
   0.00,              /* 'I' integral gain              */
   0.00,              /* 'D' derivative gain            */
   0.00,              /* 'A' acceleration feed forward  */
   0.00,              /* 'F' friction feed forward      */
   0.00,              /* 'V' velocity feed forward      */
   0.00,              /* 'H' output Hold                */
   0.00,              /* 'B' bias                       */
   0.00,              /* 'R' acceleration rate          */
   0.00,              /* 'S' set point                  */
   0.00,              /* 'T' transfer ratio             */
   0.00,              /* 'L' lag in output change       */
   0.00,              /* 'N' minimum output value       */
   0.00,              /* 'M' maximum output value       */
   0.00,              /* 'Y' repeat cycle count         */
   0.00,              /* 'W' slew limit                 */
};

/*********************************************************
 *
 * Data Type:   PARAM_PARSE structure
 *
 * Contents:    values used for entering, validating, and
 *              displaying the values in the PID_PARAMS
 *              structure type
 *
 ********************************************************/
typedef struct
{
  int token;
  double *destination;
  double min_val;
  double max_val;
  int force_to_int;
  char *param_name;
} PARAM_PARSE;

static const PARAM_PARSE parse [] =
{
  { 'A', &params.acc_ff, -100.0, 100.0,
        0, "Acceleration feed forward" },
  { 'B', &params.bias,  0.00, 100.0,
        0, "constant Bias" },
  { 'D', &params.d_gain, 0.00, 100.0,
        0, "Derivative gain" },
  { 'F', &params.fri_ff, 0.00, 100.0,
        0, "Friction feed forward" },
  { 'H', &params.hold, 0.00, 100.0,
        1, "output Hold" },
  { 'I', &params.i_gain, 0.00, 100.0,
        0, "Integral gain" },
  { 'L', &params.lag,  0.00, 1.0,
        0, "response Lag" },
  { 'M', &params.max,  0, 100.0,
        0, "Maximum output" },
  { 'N', &params.min,  -100.0, 0,
        0, "miNimum output" },
  { 'P', &params.p_gain, 0.00, 100.0,
        0, "Proportional gain" },
  { 'R', &params.accel, 0.00, 100.0,
        0, "acceleration Rate" },
  { 'S', &params.setpt,  -10000, 10000,
        1, "Set point" },
  { 'T', &params.trans,  0.10, 100,
        0, "Transfer ratio" },
  { 'V', &params.vel_ff, 0.00, 100,
        0, "Velocity feed forward" },
  { 'W', &params.slew,  0.00, 100.0,
        0, "sleW limit" },
  { 'Y', &params.cycles,  0.00, 99999999.0,
        0, "cYcle count" }
};

/*********************************************************
 *
 * Data Type:   unnamed enumeration
 *
 * Contents:    specifies the offsets of the parse[] array
 *              in order for use by ShowParams()
 *
 ********************************************************/
enum
{
  ACC_FF,
  BIAS,
  D_GAIN,
  FRI_FF,
  HOLD,
  I_GAIN,
  LAG,
  MAX,
  MIN,
  P_GAIN,
  ACCEL,
  SETPT,
  TRANS,
  VEL_FF,
  SLEW,
  CYCLES
};

#define MAX_EVENTS 100

/*********************************************************
 *
 * Data Type:   EVENT structure
 *
 * Contents:    holds events set by reading an input file,
 *              to be processed at specific times during
 *              execution of the simulation
 *
 ********************************************************/
typedef struct
{
  double            *event_dest;
  double             event_value;
  unsigned long      event_time;
  int                event_token;
} EVENT;

static EVENT events[MAX_EVENTS];

/*********************************************************
 *
 * File Scope Variables
 *
 *              event_index is shared by the ParseEvent()
 *              and CheckEvent() functions
 *              
 *              this_target and next_target would normally
 *              be internal to the ComputePID() function,
 *              but have file scope so main() can output
 *              them
 *
 *              rms_error is computed by ComputePID() and
 *              output at the end of the simulation by
 *              main()
 *
 ********************************************************/
static int          event_index             = 0;

static double       this_target             = 0.0;
static double       next_target             = 0.0;

static double       rms_error               = 0.0;

/*********************************************************
 *
 * Function:    ShowMenuItem
 *
 * Parameters:  const PARAM_PARSE *pp, pointer to a structure
 *              describing a PID_PARAMS member
 *
 * Returns:     nothing
 *
 * Description: displays the parameter and its current value
 *
 ********************************************************/
static void
ShowMenuItem(const PARAM_PARSE *pp)
{
  int     count;
  int     padding     = 28 - (int)strlen(pp->param_name);
  
  fprintf(stderr, "    [%c]  %s", pp->token, pp->param_name);

  for (count = 0; count < padding; ++count)
  {
    putc(' ', stderr);
  }

  if (pp->force_to_int)
  {
    fprintf(stderr, "%9.0f\n", *pp->destination);
  }
  else
  {
    fprintf(stderr, "%9.4f\n", *pp->destination);
  }
}

/*********************************************************
 *
 * Function:    ShowParams
 *
 * Parameters:  none
 *
 * Returns:     nothing
 *
 * Description: shows all parameters in a formatted
 *              display
 *
 ********************************************************/
static void
ShowParams(void)
{
  fputs("\nCurrent Parameter Values\n", stderr);
  ShowMenuItem(parse + P_GAIN);
  ShowMenuItem(parse + I_GAIN);
  ShowMenuItem(parse + D_GAIN);
  ShowMenuItem(parse +   HOLD);
  ShowMenuItem(parse + ACC_FF);
  ShowMenuItem(parse + FRI_FF);
  ShowMenuItem(parse + VEL_FF);
  ShowMenuItem(parse +   BIAS);
  ShowMenuItem(parse +  ACCEL);
  ShowMenuItem(parse +  SETPT);
  ShowMenuItem(parse +  TRANS);
  ShowMenuItem(parse +    LAG);
  ShowMenuItem(parse +    MIN);
  ShowMenuItem(parse +    MAX);
  ShowMenuItem(parse +   SLEW);
  ShowMenuItem(parse + CYCLES);
  fputs("\nEnter letter to change value\n\n", stderr);
}

/*********************************************************
 *
 * Function:    ClearStdin
 *
 * Parameters:  none
 *
 * Returns:     EOF or '\n'
 *
 * Description: this function reads and discards any
 *              characters remaining in stdin, up until
 *              a '\n' is discarded or EOF is found, should
 *              only be used when it is known that
 *              there is left-over input to be discarded
 *
 ********************************************************/
static int
ClearStdin(void)
{
  int               ch;
  do
  {
    ch = getchar();
  } while (ch != EOF && ch != '\n');

  return ch;
}

/*********************************************************
 *
 * Function:    SkipWhiteSpace
 *
 * Parameters:  const char *cp, a C string (array of chars
 *              terminated with '\0'
 *
 * Returns:     pointer to first non white space character
 *              if any in the string, otherwise NULL
 *
 * Description: this function is used to skip over leading
 *              white space in a string
 *
 ********************************************************/
static char
*SkipWhiteSpace(const char *cp)
{
  unsigned int      ch;

  for ( ; (ch = *(unsigned char *)cp) != 0; ++cp)
  {
    if (!isspace(ch))
    {
      return (char *)cp;
    }
  }
  return NULL;
}

/*********************************************************
 *
 * Function:    Compare
 *
 * Parameters:  const void *ip, actually pointer to an int,
 *              should contain upper case alpha character
 *
 *              const void *pp, actually pointer to a
 *              PARAM_PARSE structure
 *
 * Returns:     0 if the int matches the token in
 *              the structure
 *
 *              < 0 if the int is less than the token
 *              in the structure
 *
 *              > 0 if the int is larger than the token
 *              in the structure
 *
 * Description: this function is not called direcely but
 *              instead passed to bsearch() as the comparison
 *              function for locating a PARAM_PARSE
 *              structure matching a specific input character
 *
 ********************************************************/
static int
Compare(const void *ip, const void *pp)
{
  return *(int *)ip - ((PARAM_PARSE *)pp)->token;
}

/*********************************************************
 *
 * Function:    EventCompare
 *
 * Parameters:  const void *p1, actually a pointer to an
 *              EVENT structure
 *
 *              const void *p2, actually another pointer
 *              to an EVENT structure
 *
 * Returns:     0 if event_time member of both structures
 *              are the same
 *
 *              -1 of the event_time member of the first
 *              structure is earlier (lower numerically)
 *
 *              +1 if the event_time member of the first
 *              structure is later (higher numerically)
 *
 * Description: this function is not called direcely but
 *              instead passed to qsort() as the comparison
 *              function for sorting the array of entered
 *              events in ascending chronological order
 *
 ********************************************************/
static int
EventCompare(const void *p1, const void *p2)
{
  
  unsigned long     u1        = ((EVENT *)p1)->event_time;
  unsigned long     u2        = ((EVENT *)p2)->event_time;
  
  if (u1 < u2)
  {
    return -1;
  }
  else if (u1 > u2)
  {