home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 15 / 15.iso / s / s300 / 1.ddi / CHAP1 / CNTRL0.C next >
Encoding:
C/C++ Source or Header  |  1988-08-05  |  8.8 KB  |  319 lines
  1. /***********************************************************************
  2.  
  3. FILE
  4.     cntrl0.c  -  d.c. servo speed control example, control code
  5.  
  6. ROUTINES
  7.     control  -  execute control algorithm
  8.     gain     -  interpret command line for P gain 
  9.     setv     -  interpret command line for setpoint
  10.     getv     -  get velocity reading
  11.     mact     -  send controller output to actuator
  12.  
  13. REMARKS
  14.     implements simple P control of d.c. motor speed.
  15.  
  16. LAST UPDATE
  17.     18 March 1985  by  Haam
  18.         recast in module form
  19.     1 December 1987
  20.         restructure and modify for MSC 4.00 and 5.00
  21.     
  22.     Copyright(C) 1984-1987, D.M.Auslander and C.H. Tham
  23.     
  24. ***********************************************************************/
  25.  
  26. /***********************************************************************
  27.                           I M P O R T S
  28.  
  29.     This section contains ALL the declarations from other modules
  30.     required in this program.  Such "imports" are gathered here
  31.     instead of scattered all over this file to make them easier
  32.     to find and reduces the chance of introducing bugs through
  33.     inconsistent multiple declarations of the same item.
  34.  
  35. ***********************************************************************/
  36.  
  37. #include <stdio.h>
  38.  
  39. #include "envir.h"      /* environment definitions */
  40. #include "dash8.h"      /* declarations for dash8.c */
  41. #include "dac2.h"       /* declarations for dac2.c */
  42. #include "cntrl0.h"     /* exported declarations for this module */
  43.  
  44.  
  45. /***********************************************************************
  46.              F O R W A R D    D E C L A R A T I O N S
  47.  
  48.     In C, functions are assumed to return an integer unless declared
  49.     otherwise.  Here, the return type of all functions which do not
  50.     return integers (or anything at all) are declared.  The symbol
  51.     string "ANSI" is used by the C macro processor's conditional
  52.     compilation feature to enable "function prototyping" for
  53.     compilers that support this proposed ANSI C feature.
  54.  
  55.     This section only declares functions that are local to this
  56.     module; functions visible outside this module are already
  57.     declared in the header file cntrl0.h #include'd above.
  58.  
  59. ***********************************************************************/
  60.  
  61. #ifdef ANSI
  62.  
  63. double getv(void);
  64. void   mact(double);
  65.  
  66. #else
  67.  
  68. double getv();
  69. void   mact();
  70.  
  71. #endif
  72.  
  73.  
  74. /***********************************************************************
  75.            P R I V A T E    D A T A    V A R I A B L E S
  76.  
  77.     These variables and declarations are visible to all routines in
  78.     this file.  Variables declared as "static" are not visible
  79.     outside of this file.  In computer jargon, their "name space" is
  80.     restricted to this file.  This enhances data security by preventing
  81.     routines outside this file from inadvertently modidifying these
  82.     values though accidental use of an external variable with the
  83.     same name.
  84.  
  85. ***********************************************************************/
  86.  
  87. #define ADCHANNEL   0           /* A/D channel that we will use */
  88. #define DACHANNEL   0           /* D/A channel used by this module */
  89.  
  90. static double vref = 1000.0;    /* velocity setpoint  */
  91. static double kp = 1.0;         /* proportional gain */
  92. static double vcon = 0.0;       /* constant term in the controller */
  93. static double mmin;             /* lower limit for controller output */
  94. static double mmax;             /* upper limit for controller output */
  95.  
  96.  
  97. /***********************************************************************
  98.                   E N T R Y    R O U T I N E S
  99.  
  100.     Entry routines are those that can be called directly by functions
  101.     outside this module or file.
  102.  
  103. ***********************************************************************/
  104.  
  105. /*----------------------------------------------------------------------
  106. PROCEDURE
  107.     CONTROL  -  implements P control algorithm
  108.  
  109. SYNOPSIS
  110.     void control(cline)
  111.     char *cline;
  112.  
  113. PARAMETERS
  114.     cline  -  pointer to user's argument string
  115.  
  116. LAST UPDATE
  117.     15 October 1984
  118. ----------------------------------------------------------------------*/
  119.  
  120. void control(cline)
  121. char *cline;
  122. {
  123.     long nitr;          /* number of iterations */
  124.     long i;             /* iteration counter */
  125.     double v;           /* the motor velocity */
  126.     double m;           /* controller output */
  127.     int damin, damax;   /* output argument range of D/A */
  128.     int prnt;           /* 1 = print control output, 0 = no print */
  129.  
  130.     /*---------------------------------------------------------------
  131.         Get number of iterations and print flag from the command
  132.         line, cline.  Sscanf() is just like scanf() except that the
  133.         input is a string instead of the standard input (keyboard).
  134.     -----------------------------------------------------------------*/
  135.  
  136.     sscanf(cline, "%ld %d", &nitr, &prnt);
  137.  
  138.     printf("number of iterations = %ld\n", nitr);   /* verify */
  139.  
  140.     da_limits(&damin, &damax);      /* find D/A output limits */
  141.  
  142.     mmin = (double)damin;
  143.     mmax = (double)damax;
  144.  
  145.     for (i = 1; i <= nitr; i++)     /* main iteration loop */
  146.     {
  147.         v = getv();                 /* get motor velocity */
  148.  
  149.         m = kp * (vref - v) + vcon; /* the control equation */
  150.  
  151.         if (m > mmax)               /* limit value of m to allowable */
  152.             m = mmax;               /*   d/a and actuator range.     */
  153.         else if (m < mmin)
  154.             m = mmin;
  155.  
  156.         if (prnt)
  157.             printf("v = %f, m = %f\n", v, m);
  158.  
  159.         mact(m);        /* send controller output to the actuator. */
  160.  
  161. #ifdef SIMRT
  162.         tstep();        /* time simulation: step ahead one step in time */
  163. #endif
  164.  
  165.     }   /* end of main iteration loop. */
  166.  
  167. }
  168.  
  169.  
  170.  
  171. /*----------------------------------------------------------------------
  172. PROCEDURE
  173.     GAIN  -  interpret the input line specifying controller gain(s)
  174.  
  175. SYNOPSIS
  176.     void gain(cline)
  177.     char *cline;
  178.  
  179. PARAMETERS
  180.     cline  -  input line
  181.  
  182. LAST UPDATE
  183.     15 October 1984
  184. ----------------------------------------------------------------------*/
  185.  
  186. void gain(cline)
  187. char *cline;
  188. {
  189.  
  190.     sscanf(cline, "%lf %lf", &kp, &vcon);       /* decode input line */
  191.  
  192. #if DEBUG
  193.     printf("<gain> kp = %lf, vcon = %lf\n", kp, vcon);
  194. #endif
  195.  
  196. }
  197.  
  198.  
  199.  
  200. /*----------------------------------------------------------------------
  201. PROCEDURE
  202.     SETV  -  get velocity setpoint from the command line
  203.  
  204. SYNOPSIS
  205.     void setv(cline)
  206.     char *cline;
  207.  
  208. PARAMETERS
  209.     cline  -  user's command line
  210.  
  211. LAST UPDATE
  212.     15 October 1984  by  Haam
  213. ----------------------------------------------------------------------*/
  214.  
  215. void setv(cline)
  216. char *cline;
  217. {
  218.  
  219.     sscanf(cline,"%lf", &vref);
  220.  
  221. #if DEBUG
  222.     printf("<setv> vref = %lf\n", vref);
  223. #endif
  224.  
  225. }
  226.  
  227.  
  228.  
  229. /***********************************************************************
  230.  
  231.     The following section contains routines which interface between
  232.     high level control code and functions which perform the hardware
  233.     dependent functions of operating the A/D and D/A.  As such, they
  234.     are necessarily dependent on the device driver interface.
  235.  
  236. ***********************************************************************/
  237.  
  238. /*----------------------------------------------------------------------
  239. PROCEDURE
  240.     INIT  -  system initialization
  241.  
  242. SYNOPSIS
  243.     void init()
  244.  
  245. REMARKS
  246.     Performs any initializations required.
  247.  
  248. LAST UPDATE
  249.     1 December 1987
  250. ----------------------------------------------------------------------*/
  251.  
  252. void init(cline)
  253. char *cline;
  254. {
  255.  
  256.     ad_init();              /* initialize A/D */
  257.     da_init();              /* initialize D/A */
  258.  
  259. #ifdef SIMRT
  260.     sim_init(cline);        /* initialize simulation module */
  261. #endif
  262.  
  263. }
  264.  
  265.  
  266.  
  267. /*---------------------------------------------------------------------
  268. FUNCTION
  269.     GETV  -  returns motor velocity
  270.  
  271. SYNOPSIS
  272.     static double getv()
  273.  
  274. RETURNS
  275.     current motor velocity (units unspecified)
  276.  
  277. REMARKS
  278.     Reads channel 0 of the A/D converter, convert the value to a
  279.     double type and return this converted value.
  280.  
  281. LAST UPDATE
  282.     15 October 1984
  283. ---------------------------------------------------------------------*/
  284.  
  285. static double getv()
  286. {
  287.  
  288.     return((double)ad_wread(ADCHANNEL));
  289. }
  290.  
  291.  
  292.  
  293. /*----------------------------------------------------------------------
  294. PROCEDURE
  295.     MACT  -  send controller output to actuator
  296.  
  297. SYNOPSIS
  298.     static void mact(value)
  299.     double value;
  300.  
  301. PARAMETER
  302.     value  -  controller output
  303.  
  304. REMARKS
  305.     Convert output to integer and send it to D/A converter
  306.  
  307. LAST UPDATE
  308.     15 October 1984
  309. ----------------------------------------------------------------------*/
  310.  
  311. static void mact(value)
  312. double value;
  313. {
  314.  
  315.     da_write(DACHANNEL, (int)value);
  316. }
  317.  
  318.  
  319.