home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 15 / 15.iso / s / s300 / 1.ddi / CHAP2 / CNTRL1.C < prev    next >
Encoding:
C/C++ Source or Header  |  1988-05-02  |  9.5 KB  |  368 lines
  1. /***********************************************************************
  2.  
  3. FILE
  4.     cntrl1.c  -  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 PI control and uses a timer to determine sample
  15.     intervals.
  16.  
  17. LAST UPDATE
  18.     1 December 1987
  19.         restructure and modify for MSC 4.00 and 5.00
  20.  
  21.     Copyright(C) 1984-1987, D.M.Auslander and C.H. Tham
  22.  
  23. ***********************************************************************/
  24.  
  25. /***********************************************************************
  26.                             I M P O R T S
  27. ***********************************************************************/
  28.  
  29. #include <stdio.h>
  30.  
  31. #include "envir.h"      /* environment definitions */
  32. #include "dash8.h"      /* declarations for dash8.c */
  33. #include "dac2.h"       /* declarations for dac2.c */
  34. #include "time0.h"      /* declarations for time0.c */
  35. #include "cntrl1.h"     /* exported declarations for this module */
  36.  
  37.  
  38. /***********************************************************************
  39.              F O R W A R D    D E C L A R A T I O N S
  40. ***********************************************************************/
  41.  
  42. #ifdef ANSI
  43.  
  44. double getv(void);
  45. void   mact(double);
  46.  
  47. #else
  48.  
  49. double getv();
  50. void   mact();
  51.  
  52. #endif
  53.  
  54.  
  55. /***********************************************************************
  56.             P R I V A T E    D A T A    V A R I A B L E S
  57. ***********************************************************************/
  58.  
  59. #define ADCHANNEL   0           /* A/D channel that we will use */
  60. #define DACHANNEL   0           /* D/A channel used by this module */
  61.  
  62. static double vref = 1000.0;    /* velocity setpoint  */
  63. static double kp = 1.0;         /* proportional gain */
  64. static double ki = 0.2;         /* integral gain. */
  65. static double vcon = 0.0;       /* constant term in the controller */
  66. static double mmin;             /* lower limit for controller output */
  67. static double mmax;             /* upper limit for controller output */
  68. static double tsamp = 0.5;      /* controller sample interval (sec). */
  69. static double isum;             /* integrator accumulated value. */
  70.  
  71.  
  72. /***********************************************************************
  73.                     E N T R Y    R O U T I N E S
  74. /***********************************************************************
  75.  
  76. /*----------------------------------------------------------------------
  77. PROCEDURE
  78.     control  -  this function does the control
  79.  
  80. SYNOPSIS
  81.     void control(cline)
  82.     char *cline;
  83.  
  84. PARAMETER
  85.     cline  -  user's input line
  86.  
  87. REMARKS
  88.     Implements PI control.
  89.  
  90.     The control output is calculated in two parts - first with only
  91.     P action and add I action.  The output is limited to what the
  92.     actuator can put out, with provisions against reset windup.
  93.  
  94.     Note that the timer is set in milliseconds.
  95.  
  96. LAST UPDATE
  97.     1 December 1987
  98. ----------------------------------------------------------------------*/
  99.  
  100. void control(cline)
  101. char *cline;
  102. {
  103.     long nitr;          /* number of iterations */
  104.     long i;             /* iteration counter */
  105.     double v;           /* the motor velocity */
  106.     double m1;          /* intermediate result in control calculation. */
  107.     double m;           /* controller output */
  108.     int damin, damax;   /* output argument range of D/A */
  109.     int prnt;           /* 1 = print control output, 0 = no print */
  110.  
  111.  
  112.     /*---------------------------------------------------------------
  113.         Get number of iterations and print flag from the command
  114.         line, cline.  Sscanf() is just like scanf() except that the
  115.         input is a string instead of the standard input (keyboard).
  116.     -----------------------------------------------------------------*/
  117.  
  118.     sscanf(cline, "%ld %d", &nitr, &prnt);
  119.  
  120.     printf("number of iterations = %ld\n", nitr);   /* verify */
  121.  
  122.     da_limits(&damin, &damax);      /* find D/A output limits */
  123.  
  124.     mmin = (double)damin;
  125.     mmax = (double)damax;
  126.     
  127.     isum = 0.0;             /* initialize integrator. */
  128.  
  129.     
  130.     for (i = 0; i < nitr; i++)
  131.     {
  132.         SetTimer((long)(tsamp * 1000L));    /* set countdown timer in ms */
  133.  
  134.         v = getv();                 /* get motor velocity */
  135.  
  136.         isum += vref - v;           /* integrate error term */
  137.  
  138.         /*----------------------------------------------------------
  139.             First calculate control output without integral term.
  140.         -----------------------------------------------------------*/
  141.  
  142.         m1 = kp * (vref - v)  +  vcon;
  143.  
  144.         /*----------------------------------------------------------
  145.             Check for actuator limits and apply "reset windup".
  146.             Limit value of m to allowable D/A and actuator range
  147.             and set integral term to zero if system is outside
  148.             linear zone.
  149.         ------------------------------------------------------------*/
  150.  
  151.         if (m1 > mmax)          /* output exceeds actuator limit */
  152.         {
  153.             m1 = mmax;          /* set output to actuator maximum */
  154.             isum = 0.0;         /* set integral term to zero */
  155.         }
  156.         else if (m1 < mmin)
  157.         {
  158.             m1 = mmin;
  159.             isum = 0.0;
  160.         }
  161.     
  162.         m = m1 + ki * isum;     /* now add in integral term. */
  163.     
  164.         mact(m);                /* send control output to actuator */
  165.  
  166.         if (prnt)
  167.             printf("v = %lf, m = %lf\n", v, m);
  168.  
  169.         if (TimeUp())           /* timer has run out */
  170.         {
  171.             printf("sample time too short\n");
  172.             exit(1);
  173.         }
  174.  
  175.         while (!TimeUp())
  176.         {
  177. #if SIMRT
  178.             tstep();    /* advance time */
  179. #endif
  180.         }
  181.     }   /* end of control loop */
  182.  
  183. }
  184.  
  185.  
  186.  
  187. /*----------------------------------------------------------------------
  188. PROCEDURE
  189.     GAIN  -  interpret the input line specifying controller gain(s)
  190.  
  191. SYNOPSIS
  192.     void gain(cline)
  193.     char *cline;
  194.  
  195. PARAMETERS
  196.     cline  -  input line
  197.  
  198. LAST UPDATE
  199.     15 October 1984
  200. ----------------------------------------------------------------------*/
  201.  
  202. void gain(cline)
  203. char *cline;
  204. {
  205.  
  206.     sscanf(cline, "%lf %lf %lf", &kp, &ki, &vcon);  /* decode input line */
  207.  
  208. #if DEBUG
  209.     printf("<gain> kp = %lf, ki = %lf, vcon = %lf\n", kp, kp, vcon);
  210. #endif
  211.  
  212. }
  213.  
  214.  
  215.  
  216. /*----------------------------------------------------------------------
  217. PROCEDURE
  218.     SETV  -  get velocity setpoint from the command line
  219.  
  220. SYNOPSIS
  221.     void setv(cline)
  222.     char *cline;
  223.  
  224. PARAMETERS
  225.     cline  -  user's command line
  226.  
  227. LAST UPDATE
  228.     15 October 1984
  229. ----------------------------------------------------------------------*/
  230.  
  231. void setv(cline)
  232. char *cline;
  233. {
  234.  
  235.     sscanf(cline,"%lf", &vref);
  236.  
  237. #if DEBUG
  238.     printf("<setv> vref = %lf\n", vref);
  239. #endif
  240.  
  241. }
  242.  
  243.  
  244.  
  245. /*----------------------------------------------------------------------
  246. PROCEDURE
  247.     SMPSET  -  set sample time
  248.  
  249. SYNOPSIS
  250.     smpset(cline)
  251.     char *cline;
  252.  
  253. PARAMETER
  254.     cline  -  user's command line
  255.  
  256. DESCRIPTION
  257.     Extract sample time from user's command line and use it to set the
  258.     sample time on the timer.
  259.  
  260. LAST UPDATE
  261.     1 December 1987
  262. ----------------------------------------------------------------------*/
  263.  
  264. void smpset(cline)
  265. char *cline;
  266. {
  267.  
  268.     sscanf(cline, "%lf", &tsamp);
  269.  
  270. #if DEBUG
  271.     printf("<smpset> tsamp = %lf\n", tsamp);
  272. #endif
  273.  
  274. }
  275.  
  276.  
  277.  
  278. /***********************************************************************
  279.  
  280.     The following section contains routines which interface between
  281.     high level control code and functions which perform the hardware
  282.     dependent functions of operating the A/D and D/A.  As such, they
  283.     are necessarily dependent on the device driver interface.
  284.  
  285. ***********************************************************************/
  286.  
  287. /*----------------------------------------------------------------------
  288. PROCEDURE
  289.     INIT  -  system initialization
  290.  
  291. SYNOPSIS
  292.     void init()
  293.  
  294. REMARKS
  295.     Performs any initializations required.
  296.  
  297. LAST UPDATE
  298.     1 December 1987
  299. ----------------------------------------------------------------------*/
  300.  
  301. void init(cline)
  302. char *cline;
  303. {
  304.  
  305.     ad_init();              /* initialize A/D */
  306.     da_init();              /* initialize D/A */
  307.  
  308. #ifdef SIMRT
  309.     sim_init(cline);        /* initialize simulation module */
  310. #endif
  311.  
  312. }
  313.  
  314.  
  315.  
  316. /*---------------------------------------------------------------------
  317. FUNCTION
  318.     GETV  -  returns motor velocity
  319.  
  320. SYNOPSIS
  321.     static double getv()
  322.  
  323. RETURNS
  324.     current motor velocity (units unspecified)
  325.  
  326. REMARKS
  327.     Reads channel 0 of the A/D converter, convert the value to a
  328.     double type and return this converted value.
  329.  
  330. LAST UPDATE
  331.     1 December 1987
  332. ---------------------------------------------------------------------*/
  333.  
  334. static double getv()
  335. {
  336.  
  337.     return((double)ad_wread(ADCHANNEL));
  338. }
  339.  
  340.  
  341.  
  342. /*----------------------------------------------------------------------
  343. PROCEDURE
  344.     MACT  -  send controller output to actuator
  345.  
  346. SYNOPSIS
  347.     static void mact(value)
  348.     double value;
  349.  
  350. PARAMETER
  351.     value  -  controller output
  352.  
  353. REMARKS
  354.     Convert output to integer and send it to D/A converter
  355.  
  356. LAST UPDATE
  357.     1 December 1987
  358. ----------------------------------------------------------------------*/
  359.  
  360. static void mact(value)
  361. double value;
  362. {
  363.  
  364.     da_write(DACHANNEL, (int)value);
  365. }
  366.  
  367.  
  368.