home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 15 / 15.iso / s / s300 / 1.ddi / CHAP5 / CNTRL4.C < prev    next >
Encoding:
C/C++ Source or Header  |  1988-05-02  |  18.3 KB  |  645 lines
  1. /***********************************************************************
  2.  
  3. FILE
  4.     cntrl4.c  -  PI Control of Servo Motor Speed for Multiple Motors
  5.  
  6. ENTRY ROUTINES
  7.     control  -  execute control algorithm
  8.     init     -  interpret command line for initialization 
  9.     gain     -  interpret command line for PI gains
  10.     setv     -  interpret command line for setpoint
  11.     setsmp   -  interpret command line for sampling rate
  12.     setmot   -  allocate and initialize motor structures
  13.  
  14. PRIVATE ROUTINES
  15.     pimotor  -  apply motor control
  16.     cpmotor  -  copy motor descriptors
  17.     getv     -  get velocity reading
  18.     mact     -  send controller output to actuator
  19.  
  20. REMARKS
  21.     This version implements multiple motor control; each motor has 
  22.     completely independent sampling interval, control parameters, etc.
  23.  
  24. LAST UPDATE
  25.     20 March 1988
  26.         include ANSI features
  27.     
  28.     Copyright (c) 1984-1988  D.M.Auslander and C.H. Tham
  29.     
  30. ***********************************************************************/
  31.  
  32. /***********************************************************************
  33.                     I M P O R T S
  34. ***********************************************************************/
  35.  
  36. #include <stdio.h>
  37. #include <stdlib.h>
  38. #include <math.h>
  39.  
  40. #include "envir.h"      /* environment definitions */
  41. #include "dash8.h"      /* declarations for dash8.c */
  42. #include "dac2.h"       /* declarations for dac2.c */
  43. #include "time0.h"      /* declarations for time0.c */
  44. #include "cntrl3.h"     /* exported declarations for this module */
  45.  
  46. #include "user.h"
  47.  
  48. #ifdef SIMRT
  49. #ifdef ANSI
  50. extern void tstep(void);
  51. extern void sim_init(int);
  52. extern void sim_reset(char *);
  53. #else
  54. extern void tstep();
  55. extern void sim_init();
  56. extern void sim_reset();
  57. #endif
  58. #endif
  59.  
  60. /***********************************************************************
  61.            MODULE  PRIVATE  DATA  STRUCTURES  AND  VARIABLES
  62. ***********************************************************************/
  63.  
  64. #define ADCTOV      1.0         /* ADC units to velocity units */
  65. #define MTODAC      1.0         /* output units to DAC units */
  66.  
  67. #define TIME_GRAIN  0.001       /* left time granularity */
  68.  
  69. /*-----------------------------------------------------------*/
  70. /*  This structure describes the control and configurational */
  71. /*  parameters for velocity control of a d.c. motor.         */
  72. /*-----------------------------------------------------------*/
  73.  
  74. typedef struct motor {
  75.     int ichan;      /* A/D channel number */
  76.     int mchan;      /* D/A channel number */
  77.     double v;       /* velocity */
  78.     double vref;    /* velocity setpoint */
  79.     double kp;      /* proportional gain */
  80.     double ki;      /* integral gain */
  81.     double ival;    /* integrator accumulated value */
  82.     double vcon;    /* constant in control equation */
  83.     double m;       /* controller output */
  84.     double mmin;    /* lower limit for controller output */
  85.     double mmax;    /* upper limit for controller output */
  86.     double tsamp;   /* sample interval */
  87.     double tleft;   /* time left to next sample */
  88. } MOTOR;
  89.  
  90. static MOTOR *pm0;      /* pointer to the structure for motor #0 */
  91. static int nmotors;     /* number of motors */
  92.  
  93. /*----------------------------------------------------------------*/
  94. /*  motdef is a set of default values that is used to initialize  */
  95. /*  newly allocated motor control structure.                      */
  96. /*----------------------------------------------------------------*/
  97.  
  98. static MOTOR motdef = {
  99.           0,    /* A/D channel defaults to 0 */
  100.           0,    /* D/A channel defaults to 0 */
  101.         0.0,    /* zero initial velocity */
  102.      1000.0,    /* default velocity setpoint */
  103.         1.0,    /* kp defaults to 1.0 */
  104.         0.0,    /* no integrator by default, P control by default */
  105.         0.0,    /* zero integration sum */
  106.         0.0,    /* constant term in control equation */
  107.         0.0,    /* no controller output initially */
  108.     -2048.0,    /* lower output limit */
  109.      2047.0,    /* upper output limit */
  110.         0.5,    /* default sampling interval of 0.5 seconds */
  111.        0.0      /* time left to next sample */
  112. };
  113.  
  114. /***********************************************************************
  115.                F O R W A R D    D E C L A R A T I O N S
  116. ***********************************************************************/
  117.  
  118. #ifdef ANSI
  119.  
  120. double  getv(int);
  121. void    mact(int, double);
  122. void    pimotor(MOTOR *);
  123. void    cpmotor(MOTOR *, MOTOR *);
  124.  
  125. #else
  126.  
  127. double  getv();
  128. void    mact();
  129. void    pimotor();
  130. void    cpmotor();
  131.  
  132. #endif
  133.  
  134. /***********************************************************************
  135.                     E N T R Y    R O U T I N E S
  136. ***********************************************************************/
  137.  
  138. /*----------------------------------------------------------------------
  139. PROCEDURE
  140.     CONTROL  -  this function does the control
  141.  
  142. SYNOPSIS
  143.     void control(cline)
  144.     char cline[];
  145.  
  146. PARAMETER
  147.     cline  -  user's input line that specifies the number of iterations
  148.               and whether to trace the control action.
  149.  
  150. REMARKS
  151.     The control output is calculated in two parts - first with only P
  152.     action and then with I action added.  The output is limited to what
  153.     the actuator can put out, with provisions against reset windup.
  154.  
  155. LAST UPDATE
  156.     20 March 1988
  157.         use ANSI features
  158. ----------------------------------------------------------------------*/
  159.  
  160. void control(cline)
  161. char *cline;
  162. {
  163.     MOTOR *pm;          /* pointer to motor data structure */
  164.     long i;             /* iteration counter */
  165.     int m;              /* motor number index */
  166.  
  167.  
  168.     prologue();         /* prepare scheduling kernel */
  169.  
  170.     pm = pm0;           /* set pm to point at first motor structure */
  171.  
  172.     for (m = 0; m < nmotors; m++)
  173.     {
  174.         pm->ival = 0.0;     /* initialize the integrator */
  175.  
  176.         SetCntrlTask("motor", pimotor, (void *)pm, 1000,
  177.                         (long)(pm->tsamp * 1000.0  +  0.5));
  178.  
  179.         pm++;               /* next motor */
  180.     }
  181.  
  182.     start();            /* start the scheduler */
  183.         
  184. } 
  185.  
  186.  
  187.  
  188. /*----------------------------------------------------------------------
  189. PROCEDURE
  190.     INIT  -  process system initialization command
  191.  
  192. SYNOPSIS
  193.     void init(cline)
  194.     char *cline;
  195.  
  196. PARAMETER
  197.     cline  -  data part of input command line, specifying step size
  198.  
  199. REMARKS
  200.     This routine must be called before control() is called.
  201.  
  202. LAST UPDATE
  203.     20 March 1988
  204.         change sim_init()
  205. ----------------------------------------------------------------------*/
  206.  
  207. void init(cline)
  208. char *cline;
  209. {
  210.  
  211.     ad_init();              /* initialize A/D */
  212.     da_init();              /* initialize D/A */
  213.  
  214. #ifdef SIMRT
  215.     sim_reset(cline);
  216. #endif
  217.  
  218. }
  219.  
  220.  
  221.  
  222. /*----------------------------------------------------------------------
  223. PROCEDURE
  224.     GAIN  -  interpret input line for controller gains specification
  225.  
  226. SYNOPSIS
  227.     void gain(cline)
  228.     char *cline;
  229.  
  230. PARAMETER
  231.     cline  -  pointer to null terminated input command line
  232.  
  233. REMARKS
  234.     Note that in gain(), setv() and setsmp(), the input data is put
  235.     into temporary storage first.  This is because the specified
  236.     motor number may be out of range.
  237.  
  238. LAST UPDATE
  239.     20 March 1988
  240.         use ANSI features
  241. ----------------------------------------------------------------------*/
  242.  
  243. void gain(cline)
  244. char *cline;
  245. {
  246.     int mn;                 /* motor number */
  247.     double kp, ki, vcon;    /* temporary gain and constant terms */
  248.  
  249.  
  250.     if (sscanf(cline, "%d %lf %lf %lf", &mn, &kp, &ki, &vcon) == 4)
  251.     {
  252.         if ((mn >= 0) && (mn < nmotors))
  253.         {
  254.             /*
  255.              *  Set the appropriate fields of the motor descriptor.
  256.              *  Note that (pm0 + mn) is a pointer to the descriptor
  257.              *  for the mn-th motor; hence the use of "->" to
  258.              *  dereference the descriptor fields.
  259.              */
  260.  
  261.             (pm0 + mn)->kp = kp;
  262.             (pm0 + mn)->ki = ki;
  263.             (pm0 + mn)->vcon = vcon;
  264.  
  265. #ifdef DEBUG
  266.             printf("<gain> motor = %d, ", mn);
  267.             printf("kp = %lf, ki = %lf, vcon = %lf\n", kp, ki, vcon);
  268. #endif
  269.         }
  270.         else
  271.             printf("gain: invalid motor number: %d\n", mn);
  272.     }
  273.     else
  274.         printf("gain: invalid or insufficient parameters\n");
  275.  
  276. }
  277.  
  278.  
  279.  
  280. /*----------------------------------------------------------------------
  281. PROCEDURE
  282.     SETV  -  get velocity setpoint from command line
  283.  
  284. SYNOPSIS
  285.     void setv(cline)
  286.     char *cline;
  287.  
  288. PARAMETER
  289.     cline  -  pointer to null terminated input command line
  290.  
  291. LAST UPDATE
  292.     20 March 1988
  293.         use ANSI features
  294. ----------------------------------------------------------------------*/
  295.  
  296. void setv(cline)
  297. char *cline;
  298. {
  299.     int mn;         /* motor number */
  300.     double vref;    /* temporary holder for setpoint value */
  301.  
  302.  
  303.     if (sscanf(cline, "%d %lf", &mn ,&vref) == 2)
  304.     {
  305.         if ((mn >= 0) && (mn < nmotors))
  306.         {
  307.             (pm0 + mn)->vref = vref;        /* record velocity reference */
  308.  
  309. #ifdef DEBUG
  310.             printf("<setv> motor = %d, vref = %lf\n", mn, vref);
  311. #endif
  312.         }
  313.         else
  314.             printf("setv: bad motor number: %d\n", mn);
  315.     }
  316.     else
  317.         printf("setv: invalid or insufficient parameters\n");
  318.  
  319. }
  320.  
  321.  
  322.  
  323. /*----------------------------------------------------------------------
  324. PROCEDURE
  325.     SETSMP  -  set sampling interval from command line specification
  326.  
  327. SYNOPSIS
  328.     void setsmp(cline)
  329.     char *cline;
  330.  
  331. PARAMETER
  332.     cline  -  pointer to null terminated input command line
  333.  
  334. LAST UPDATE
  335.     20 March 1988
  336.         use ANSI features
  337. ----------------------------------------------------------------------*/
  338.  
  339. void setsmp(cline)
  340. char *cline;
  341. {
  342.     int mn;             /* motor number */
  343.     double tsamp;       /* temporary holder for sample time */
  344.  
  345.  
  346.     if (sscanf(cline,"%d %lf", &mn, &tsamp) == 2)
  347.     {
  348.         if ((mn >= 0) && (mn < nmotors))
  349.         {
  350.             (pm0 + mn)->tsamp = tsamp;      /* set sample interval */
  351.  
  352. #ifdef DEBUG
  353.             printf("<setsmp> motor = %d, tsamp = %f\n", mn, tsamp);
  354. #endif
  355.         }
  356.         else
  357.             printf("setsmp: bad motor number: %d\n", mn);
  358.     }
  359.     else
  360.         printf("setsmp: invalid or insufficient parameters\n");
  361.  
  362. }
  363.  
  364.  
  365.  
  366. /*----------------------------------------------------------------------
  367. PROCEDURE
  368.     SETMOT  -  allocate and initialize data structures for the number
  369.                 of motors requested by the user.
  370. SYNOPSIS
  371.     void setmot(void)
  372.  
  373. REMARKS
  374.     This routine must be called as the first action of the main program.
  375.  
  376.     Motor descriptors initialized from defaults specified in "motdef"
  377.  
  378. LAST UPDATE
  379.     20 March 1988
  380.         use ANSI features
  381. ----------------------------------------------------------------------*/
  382.  
  383. void setmot()
  384. {
  385.     MOTOR *pm;          /* pointer to motor data structures */
  386.     int nm;             /* number of motors */
  387.     char cbuf[20];      /* input line */
  388.     int damin, damax;   /* D/A output limits */
  389.     int done;           /* flag, if set, o.k. to return */
  390.     int i;              /* loop index */
  391.  
  392.  
  393.     do
  394.     {
  395.         done = 1;
  396.  
  397.         /*
  398.          *  Prompt user and get response line.  Because scanf() does not
  399.          *  remove the newline character at the end of lines, fgets() is
  400.          *  used instead since the newline character may cause trouble
  401.          *  with other console functions.
  402.          */
  403.  
  404.         fputs("How many motors? ", stdout);
  405.         fgets(cbuf, 20, stdin);         /* get at most 19 characters */
  406.  
  407.         if (sscanf(cbuf, "%d", &nm) == 1)   /* decode input */
  408.         {
  409.             da_limits(&damin, &damax);      /* find DAC limits */
  410.  
  411.             motdef.mmin = (double)damin / MTODAC;   /* set output limits */
  412.             motdef.mmax = (double)damax / MTODAC;
  413.  
  414.             /*
  415.              *  Dynamically allocate memory for the required number of
  416.              *  motor structures.  Note that calloc() is used instead of
  417.              *  malloc() since calloc() is suppose to guarantee that the
  418.              *  allocated block of memory has the correct alignment.
  419.              *  Note also the use of "casts" in the allocation statement;
  420.              *  though not strictly necessary, it is recommended as good
  421.              *  programming style.
  422.              */
  423.  
  424.             if ((pm0 = (MOTOR *)calloc(nm, sizeof(MOTOR))) == (MOTOR *)NULL)
  425.             {
  426.                 printf("INSUFFICIENT MEMORY: ");
  427.                 printf("cannot allocate %d motor blocks\n", nm);
  428.                 exit(1);
  429.             }
  430.  
  431.             for (i = 0, pm = pm0; i < nm; i++, pm++)
  432.             {
  433.                 cpmotor(&motdef, pm);   /* init with defaults from motdef */
  434.             
  435.                 pm->ichan = i;  /* set both A/D and D/A channels to i; the */
  436.                 pm->mchan = i;  /*   actual channels depends on your setup */
  437.             }
  438.  
  439.             nmotors = nm;       /* record number of motor descriptors */
  440.  
  441. #ifdef SIMRT
  442.             sim_init(nm);   /* if real-time simulation is to be used, */
  443. #endif                      /*   initialize the simulation module.    */
  444.  
  445.             if (nm == 1)
  446.                 printf("\nmotor 0 has been initialized\n");
  447.             else
  448.                 printf("\nmotors 0 to %d have been initialized\n", nm - 1);
  449.         }
  450.         else
  451.         {
  452.             printf("invalid entry\n");
  453.             done = 0;
  454.         }
  455.     }
  456.     while (!done);
  457.  
  458. }
  459.  
  460.  
  461. /***********************************************************************
  462.                     P R I V A T E    R O U T I N E S 
  463. ***********************************************************************/
  464.  
  465. /*----------------------------------------------------------------------
  466. PROCEDURE
  467.     PIMOTOR  -  apply PI control for motor
  468.  
  469. SYNOPSIS
  470.     static void pimotor(pm, prnt)
  471.     MOTOR *pm;
  472.     int prnt;
  473.  
  474. PARAMETERS
  475.     pm    -  pointer to motor structure requiring control
  476.     prnt  -  if 1, print control output
  477.  
  478. LAST UPDATE
  479.     20 March 1988
  480.         use ANSI features
  481.     10 April 1988
  482.         modify for clotho background scheduler
  483. ----------------------------------------------------------------------*/
  484.  
  485. static void pimotor(pm)
  486. MOTOR *pm;
  487. {
  488.     double m1;      /* intermediate result in control calculation. */
  489.     double error;   /* error between reference and measured */
  490.     int mn;         /* motor number */
  491.  
  492.  
  493.     pm->v = getv(pm->ichan);            /* obtain motor velocity */
  494.  
  495.     error = pm->vref - pm->v;           /* calculate error term */
  496.     pm->ival += error;                  /* update integral of error */
  497.     m1 = pm->kp * error + pm->vcon;     /* do P control first */
  498.  
  499.     /*
  500.      *  Check for actuator limits and apply "reset windup" control.
  501.      *  If the output would exceed actuator limits, set the integral
  502.      *  term to zero to prevent the integral term from dominating
  503.      *  the output and slow down corrective action.
  504.      */
  505.  
  506.     if (m1 > pm->mmax)
  507.     {
  508.         pm->m = pm->mmax;
  509.         pm->ival = 0.0;
  510.     }
  511.     else if (m1 < pm->mmin)
  512.     {
  513.         pm->m = pm->mmin;
  514.         pm->ival = 0.0;
  515.     }
  516.     else    /* output withing actuator limits */
  517.     {
  518.         pm->m = m1 + (pm->ki * pm->ival);   /* now add integral term */
  519.     }
  520.  
  521.     mn = (pm - pm0);    /* compute motor number for output */
  522.  
  523.     printf("motor = %d, v = %7.3lf, m = %7.3lf\n", mn, pm->v, pm->m);
  524.  
  525.     mact(pm->mchan, pm->m); /* send controller output to actuator */
  526.  
  527. }
  528.  
  529.  
  530.  
  531. /*----------------------------------------------------------------------
  532. PROCEDURE
  533.     CPMOTOR  -  copy one motor descriptor to another
  534.  
  535. SYNOPSIS
  536.     static void cpmotor(from, to)
  537.     MOTOR *from, *to;
  538.  
  539. PARAMETERS
  540.     from  -  pointer to source
  541.     to    -  pointer to destination
  542.  
  543. REMARKS
  544.     Standard (K&R) C does not allow entire structures to be assigned
  545.     (this will change with the proposed ANSI X3J11 standards).  This
  546.     restriction is very inconvenient and this routine gets around this
  547.     by doing a byte-by-byte copy of the descriptor contents.  This
  548.     technique may not work on all computers, but it is reasonably
  549.     robust and is much easier than doing field by field assignments.
  550.  
  551.     If you are using an ANSI conforming C compiler, just do a structure
  552.     assignment.  The compiler will take care of the rest.
  553.  
  554. LAST UPDATE
  555.     20 March 1988
  556.         use ANSI features
  557. ----------------------------------------------------------------------*/
  558.  
  559. static void cpmotor(from, to)
  560. MOTOR *from, *to;
  561. {
  562.  
  563. #ifdef ANSI     /* can do structure assignment */
  564.  
  565.     *to = *from;
  566.  
  567. #else           /* do explicit byte by byte copy */
  568.  
  569.     register char *src, *dst;   /* source and destination pointers */
  570.     unsigned count;             /* byte count */
  571.  
  572.  
  573.     src = (char *)from;     /* make fast copy of source pointer */
  574.     dst = (char *)to;       /* make fast copy of destination pointer */
  575.  
  576.     for (count = 0; count < sizeof(MOTOR); count++)
  577.         *dst++ = *src++;
  578.  
  579. #endif
  580.  
  581. }
  582.  
  583.  
  584.  
  585. /*----------------------------------------------------------------------
  586. FUNCTION
  587.     GETV  -  get current motor velocity
  588.  
  589. SYNOPSIS
  590.     static double getv(channel)
  591.     int channel;
  592.  
  593. PARAMETER
  594.     channel  -  A/D channel from which to read the velocity
  595.  
  596. RETURNS
  597.     current motor velocity
  598.  
  599. REMARKS
  600.     Note that the conversion of raw ADC units to velocity units
  601.     uses the ADCTOV conversion factor.
  602.  
  603. LAST UPDATE
  604.     20 March 1988
  605.         use ANSI features
  606. ----------------------------------------------------------------------*/
  607.  
  608. static double getv(channel)
  609. int channel;
  610. {
  611.  
  612.     return((double)ad_wread(channel) * ADCTOV);
  613. }
  614.  
  615.  
  616.  
  617. /*----------------------------------------------------------------------
  618. PROCEDURE
  619.     mact  -  send controller output to D/A converter
  620.  
  621. SYNOPSIS
  622.     static void mact(channel, output)
  623.     int channel;
  624.     double output;
  625.  
  626. PARAMETERS
  627.     channel -  D/A channel number
  628.     output  -  controller output
  629.  
  630. REMARKS
  631.     Multiply by conversion factor and add 0.5 for rounding.
  632.  
  633. LAST UPDATE
  634.     20 March 1988
  635.         use ANSI features
  636. ----------------------------------------------------------------------*/
  637.  
  638. static void mact(channel, output)
  639. int channel;
  640. double output;
  641. {
  642.     
  643.     da_write(channel, (int)((output * MTODAC) + 0.5));
  644. }
  645.