home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 15 / 15.iso / s / s300 / 1.ddi / CHAP3 / CASCADE0.C < prev    next >
Encoding:
C/C++ Source or Header  |  1988-05-02  |  11.0 KB  |  438 lines
  1. /*********************************************************************
  2.  
  3. FILE
  4.     cascade0.c  -  cascade PID position and velocity control
  5.  
  6. ROUTINES
  7.     control  -  the cascade controller
  8.     pgain    -  get position gains
  9.     vgain    -  get velocity gains
  10.     smpset   -  set sampling interval
  11.     getv     -  get velocity
  12.     getp     -  get position
  13.  
  14. REMARKS
  15.     This implements the cascade control configuration of figure 3.7
  16.     in the book.  There are two control loops, an inner velocity
  17.     loop and an outer position loop.  The input to the velocity loop
  18.     is the output of the position loop.
  19.  
  20.     This example demonstrates the use of time2.c, quad1.c and a
  21.     modified pwm.c for timing and i/o interface.  You need to modify
  22.     pwm.c in order to use it with quad1.c (refer to remarks for
  23.     the mact() routine).
  24.  
  25.     This file contains only the control implementation.  You have
  26.     to implement the user interface and perhaps the simulation
  27.     module.  For examples of user interface and simulation modules,
  28.     you can refer to cntrl1.c and rtsim1.c.
  29.  
  30. LAST UPDATE
  31.     27 January 1988
  32.         use quad1.c and pwm.c
  33.  
  34. ***********************************************************************/
  35.     
  36. /***********************************************************************
  37.                             I M P O R T S
  38. ***********************************************************************/
  39.  
  40. #include <stdio.h>
  41.  
  42. #include "envir.h"          /* environment definitions */
  43.  
  44. #include "time2.h"
  45. #include "quad1.h"
  46. #include "pwm.h"
  47.  
  48.  
  49. /***********************************************************************
  50.                 F O R W A R D    D E C L A R A T I O N S
  51. ***********************************************************************/
  52.  
  53. #ifdef ANSI
  54.  
  55. double  getv(void);
  56. double  getp(void);
  57. void    mact(double);
  58.  
  59. #else
  60.  
  61. double  getv();
  62. double  getp();
  63. void    mact();
  64.  
  65. #endif
  66.  
  67. /**********************************************************************
  68.                     P R I V A T E     D A T A
  69. **********************************************************************/
  70.  
  71. #define SCALE   1000.0      /* quadrature states per unit displacement */
  72. #define PWMFREQ 50.0        /* pwm frequency */
  73. #define TMRFREQ 50L         /* timer "tick" frequency in milliseconds */
  74.  
  75. #define LOOPTIMER   0       /* timer number for control loop */
  76. #define PWMTIMER    1       /* timer number for pwm pulse timing */
  77.  
  78. static double mvmax =  2047.0;  /* max velocity actuator output */
  79. static double mvmin = -2048.0;  /* min velocity actuator output */
  80.  
  81. static double tsamp = 0.5;      /* sampling interval */
  82.  
  83. static double Kpp, Kpi, Kpd;    /* position P, I and D gains */
  84. static double Kvp, Kvi, Kvd;    /* velocity P, I and D gains */
  85. static double pref;             /* position set point */
  86. static double perr0;            /* position error at current time k */
  87. static double perr1;            /* position error at time (k - 1) */
  88. static double perr2;            /* position error at time (k - 2) */
  89. static double verr0;            /* velocity error at current time k */
  90. static double verr1;            /* velocity error at time (k - 1) */
  91. static double verr2;            /* velocity error at time (k - 2) */
  92.  
  93.  
  94. /**********************************************************************
  95.                     E N T R Y     R O U T I N E S
  96. **********************************************************************/
  97.  
  98. /*----------------------------------------------------------------------
  99. PROCEDURE
  100.     CONTROL  -  implements cascade position-velocity control
  101.  
  102. SYNOPSIS
  103.     void control(cline)
  104.     char *cline;
  105.  
  106. PARAMETER
  107.     cline  -  pointer to input line.
  108.  
  109. REMARKS
  110.     The error terms are initialized to zero.  When this routine starts,
  111.     it is equivalent to applying a step position reference input.
  112.     Note also that mp and mv, the position and velocity outputs, must 
  113.     be initialized too.
  114.  
  115. LAST UPDATE
  116.     1 April 1985
  117. ----------------------------------------------------------------------*/
  118.  
  119. void control(cline)
  120. char *cline;
  121. {
  122.     double p;           /* position */
  123.     double v;           /* velocity */
  124.     double mp;          /* postion controller output */
  125.     double delta_mp;    /* change in mp */
  126.     double mv;          /* velocity controller output */
  127.     double delta_mv;    /* change in mv */
  128.     long i;             /* iteration counter */
  129.     long nitr;          /* # of iterations */
  130.     int prnt;           /* 1 - print control output, 0 - do not print */
  131.  
  132.  
  133.     /*  get user specified iteration limit and print switch  */
  134.  
  135.     sscanf(cline, "%ld %d", &nitr, &prnt);
  136.  
  137. #ifdef DEBUG
  138.     printf("number of iterations = %ld\n", nitr);
  139. #endif
  140.     
  141.     /* initialize position and velocity error variables */
  142.  
  143.     perr0 = perr1 = perr2 = 0.0;
  144.     verr0 = verr1 = verr2 = 0.0;
  145.     mp = 0.0;
  146.     mv = 0.0;
  147.  
  148.  
  149.     for (i = 0L; i < nitr; i++)
  150.     {
  151.         SetDTimer(LOOPTIMER, (long)(tsamp * 1000));
  152.  
  153.         /* calculate position control parameters */
  154.  
  155.         perr2 = perr1;          /* update position errors */
  156.         perr1 = perr0;
  157.  
  158.         p = getp();             /* read position */
  159.  
  160.         perr0 = pref - p;       /* calculate new position error */
  161.  
  162.         delta_mp = Kpp * (perr0 - perr1)  +  Kpi * perr0
  163.                         +  Kpd * (perr0  -  2.0 * perr1  +  perr2);
  164.         
  165.         mp += delta_mp;
  166.  
  167.  
  168.         /* calculate velocity control parameters */
  169.  
  170.         verr2 = verr1;          /* update velocity errors */
  171.         verr1 = verr0;
  172.  
  173.         v = getv();             /* read velocity */
  174.  
  175.         verr0 = mp - v;         /* calculate new velocity error */
  176.  
  177.         delta_mv = Kvp * (verr0 - verr1)  +  Kvi * verr0
  178.                         +  Kvd * (verr0  -  2.0 * verr1  +  verr2);
  179.         
  180.         mv += delta_mv;
  181.  
  182.     /*  Check for actuator limits and apply "reset windup" control.  */
  183.     /*  Limit value of m to allowable D/A and actuator range and     */
  184.     /*  set integral term to zero if system is outside linear zone.  */
  185.     
  186.         if (mv > mvmax)         /* output exceeds actuator limit */
  187.             mv = mvmax;         /* set output to actuator maximum */
  188.         else if (mv < mvmin)
  189.             mv = mvmin;
  190.     
  191.         mact(mv);       /* send controller output to the actuator. */
  192.  
  193.         if (prnt)
  194.             printf("p = %f, v = %f, mp = %f,  mv = %f\n", p, v, mp, mv);
  195.  
  196.         if (TimeUp(LOOPTIMER))
  197.         {
  198.             printf("sample time too short\n");
  199.             exit(1);
  200.         }
  201.  
  202.         while (!TimeUp(LOOPTIMER))
  203.         {
  204. #ifdef SIMRT
  205.             tstep();    /* advance time */
  206. #endif
  207.         }
  208.     }   /* end of iteration loop. */
  209.  
  210. }
  211.  
  212.  
  213.  
  214. /*----------------------------------------------------------------------
  215. PROCEDURES
  216.     PGAIN  -  interpret the input line specifying position gains
  217.     VGAIN  -  interpret the input line specifying velocity gains
  218.  
  219. SYNOPSIS
  220.     void pgain(cline)
  221.     char *cline;
  222.  
  223.     void vgain(cline)
  224.     char *cline;
  225.  
  226. PARAMETERS
  227.     cline  -  input command line
  228.  
  229. LAST UPDATE
  230.     26 January 1988
  231.         change to void return
  232. ----------------------------------------------------------------------*/
  233.  
  234. void pgain(cline)
  235. char *cline;
  236. {
  237.  
  238.     sscanf(cline, "%lf %lf %lf", &Kpp, &Kpi, &Kpd);
  239.  
  240. #ifdef DEBUG
  241.     printf("<pgain> Kpp = %lf, Kpi = %lf, Kpd = %lf\n", Kpp, Kpi, Kpd);
  242. #endif
  243.  
  244. }
  245.  
  246.  
  247. void vgain(cline)
  248. char *cline;
  249. {
  250.  
  251.     sscanf(cline, "%lf %lf %lf", &Kvp, &Kvi, &Kvd);
  252.  
  253. #ifdef DEBUG
  254.     printf("<vgain> Kvp = %lf, Kvi = %lf, Kvd = %lf\n", Kvp, Kvi, Kvd);
  255. #endif
  256.  
  257. }
  258.  
  259.  
  260.  
  261. /*----------------------------------------------------------------------
  262. PROCEDURE
  263.     SETP  -  get position setpoint from the command line
  264.  
  265. SYNOPSIS
  266.     void setp(cline)
  267.     char *cline;
  268.  
  269. PARAMETERS
  270.     cline  -  user's command line
  271.  
  272. LAST UPDATE
  273.     26 January 1988
  274.         change to void return type
  275. ----------------------------------------------------------------------*/
  276.  
  277. void setp(cline)
  278. char *cline;
  279. {
  280.  
  281.     sscanf(cline,"%lf", &pref);
  282.  
  283. #ifdef DEBUG
  284.     printf("<setv> pref = %lf\n", pref);
  285. #endif
  286.  
  287. }
  288.  
  289.  
  290.  
  291. /*----------------------------------------------------------------------
  292. PROCEDURE
  293.     SMPSET  -  set sample time
  294.  
  295. SYNOPSIS
  296.     void smpset(cline)
  297.     char *cline;
  298.  
  299. PARAMETER
  300.     cline  -  user's command line
  301.  
  302. REMARKS
  303.     Extract sample time from user's command line and use it to set the
  304.     sample time on the timer.
  305.  
  306. LAST UPDATE
  307.     26 January 1988
  308.         change to void return type
  309. ----------------------------------------------------------------------*/
  310.  
  311. void smpset(cline)
  312. char *cline;
  313. {
  314.  
  315.     sscanf(cline,"%lf", &tsamp);
  316.  
  317. #ifdef DEBUG
  318.     printf("<smpset> tsamp = %lf\n", tsamp);
  319. #endif
  320.  
  321. }
  322.  
  323.  
  324. /***********************************************************************
  325.                 P R I V A T E     R O U T I N E S
  326. ***********************************************************************/
  327.  
  328. /*---------------------------------------------------------------------
  329. FUNCTIONS
  330.     GETV  -  returns motor velocity
  331.     GETP  -  returns motor position
  332.  
  333. SYNOPSIS
  334.     static double getv()
  335.     static double getv()
  336.  
  337. RETURNS
  338.     current motor velocity and position (units unspecified)
  339.  
  340. REMARKS
  341.     This example shows how easy it is to use quad1.c (or any other
  342.     method) for position and velocity.  You can plug in other methods
  343.     just as easily.
  344.  
  345. LAST UPDATE
  346.     27 January 1988
  347.         show use of quad1.c instead of ADC and DAC.
  348. ---------------------------------------------------------------------*/
  349.  
  350. static double getv()
  351. {
  352.  
  353.     return(quad_getv());
  354. }
  355.  
  356.  
  357. static double getp()
  358. {
  359.  
  360.     return(quad_getp());
  361. }
  362.  
  363.  
  364.  
  365. /*----------------------------------------------------------------------
  366. PROCEDURE
  367.     MACT  -  send out controller output
  368.  
  369. SYNOPSIS
  370.     static void mact(output)
  371.     double output;
  372.  
  373. PARAMETERS
  374.     output  -  controller output
  375.  
  376. REMARKS
  377.     This example shows the use of PWM output using pwm.c
  378.     
  379.     Note that the control algorithm has already limited the output to
  380.     between mvmax and mvmin and that mvmin is negative.
  381.  
  382. LAST UPDATE
  383.     26 January 1988
  384.         substitute pwm for DAC
  385. ----------------------------------------------------------------------*/
  386.  
  387. static void mact(output)
  388. double output;
  389. {
  390.     double dutycycle;
  391.  
  392.  
  393.     if (output >= 0.0)
  394.     {
  395.         dutycycle = output / mvmax;
  396.     }
  397.     else
  398.     {
  399.         dutycycle = -(output / mvmin);
  400.     }
  401.  
  402.     setdcycle(dutycycle);
  403.     
  404. }
  405.  
  406.  
  407.  
  408. /*----------------------------------------------------------------------
  409. PROCEDURE
  410.     INIT  -  system initialization
  411.  
  412. SYNOPSIS
  413.     void init()
  414.  
  415. REMARKS
  416.     Initialize the time2.c, quad1.c and the yet to be written pwm1.c.
  417.  
  418. LAST UPDATE
  419.     27 January 1988
  420.         initialize quad1 and a modified pwm.
  421. ----------------------------------------------------------------------*/
  422.  
  423. void init(cline)
  424. char *cline;
  425. {
  426.  
  427.     InitTime(TMRFREQ);          /* init time.c module */
  428.  
  429.     quad_init(SCALE, tsamp);
  430.  
  431.     pwm_init(PWMTIMER);
  432.     setpfreq(PWMFREQ);
  433.  
  434. }
  435.  
  436.  
  437.  
  438.