home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 15 / 15.iso / s / s300 / 1.ddi / CHAP5 / RTSIM3.C < prev    next >
Encoding:
C/C++ Source or Header  |  1988-05-02  |  12.4 KB  |  502 lines
  1. /***********************************************************************
  2.  
  3. FILE
  4.     rtsim3.c  -  simulation module for cntrl3.c
  5.  
  6. ENTRY ROUTINES
  7.     tstep      -  advance one time step
  8.     SetTimer   -  set count-down timer
  9.     TimeUp     -  has count down completed?
  10.  
  11.     ad_init    -  simulates A/D initialization
  12.     ad_start   -  simulates starting an A/D read
  13.     ad_read    -  simulates reading A/D value
  14.     ad_wread   -  simulates read from A/D
  15.  
  16.     da_init    -  simulates D/A initialization
  17.     da_limits  -  returns simulation D/A output limits
  18.     da_write   -  simulates write to D/A
  19.  
  20.     sim_reset  -  reset simulation parameters
  21.  
  22. PRIVATE ROUTINE
  23.     simul      -  motor plant simulation
  24.  
  25. INITIALIZATION ROUTINE
  26.     sim_init   -  initialize this module
  27.  
  28. REMARKS
  29.     Implements multiple motor simulation for main3.c and cntrl3.c.
  30.  
  31.     We adopt the convention that the ADC and DAC channel number refers
  32.     to the motor number, i.e., its index in the array of motor simulation
  33.     descriptors.  For example, (pmsim0 + 0) points to the simulated
  34.     motor "connected" to ADC and DAC channel 0; (pmsim0 + 2) points to
  35.     the motor "connected" to ADC and DAC channel 2.  Of course, the
  36.     ADC and DAC channels must be the same.
  37.  
  38. LAST UPDATE
  39.     22 March 1985
  40.         recast in module format.
  41.     20 March 1988
  42.         use ANSI features
  43.     10 April 1988
  44.         copy timer simulation from rtsim2.c
  45.  
  46.     Copyright (C) 1984-1988  D.M. Auslander and C.H. Tham
  47.  
  48. ***********************************************************************/
  49.  
  50. /***********************************************************************
  51.                             I M P O R T S
  52. ***********************************************************************/
  53.  
  54. #include <stdio.h>
  55. #include <stdlib.h>
  56.  
  57. #include "envir.h"      /* environment specifications */
  58. #include "dash8.h"      /* declarations for dash8.c */
  59. #include "dac2.h"       /* declarations for dac2.c */
  60. #include "time0.h"      /* declarations for time0.c */
  61.  
  62. /***********************************************************************
  63.                F O R W A R D    D E C L A R A T I O N S
  64. ***********************************************************************/
  65.  
  66. #ifdef ANSI
  67.  
  68. extern void tstep(void);        /* these are referened outside of */
  69. extern void sim_init(int);      /*   this simulation module.      */
  70. extern void sim_reset(char *);
  71.  
  72. void simul(void);               /* these are not referenced outside */
  73. void runtimer(void);            /*   of this simulation module.     */
  74.  
  75. #else
  76.  
  77. extern void tstep();
  78. extern void sim_init();
  79. extern void sim_reset();
  80.  
  81. void simul();
  82. void runtimer();
  83.  
  84. #endif
  85.  
  86. /**********************************************************************
  87.                  P R I V A T E    D A T A
  88. **********************************************************************/
  89.  
  90. #define VEL_SCALE       512.0   /* scale speed to A/D units */
  91.  
  92. #define ADMAX        2047       /* max A/D input value */
  93. #define ADMIN       -2048       /* min A/D input value */
  94.  
  95. #define DASCALE      2047.0     /* scale D/A value to torque */
  96. #define DAMIN       -2048       /* min. output value accepted */
  97. #define DAMAX        2047       /* max. output value accepted */
  98.  
  99. static double t;                /* time */
  100. static double dt;               /* simulation step size in seconds */
  101.  
  102. static long freq = 1000L;       /* timer frequency (ticks/sec) */
  103. static long ticks;              /* how many ticks to timeout */
  104. static int tflag;               /* set to 1 if timer runs out */
  105.  
  106. static int initflag = 0;        /* indicates if module initialized */
  107.  
  108. typedef struct motsim { /* motor simulation control structure */
  109.     double inertia;             /* motor inertia */
  110.     double speed;               /* motor speed */
  111.     double torque;              /* applied torque */
  112. } MOTSIM;
  113.  
  114. static MOTSIM *pmsim0;      /* pointer to first motor simulation block */
  115.  
  116. static int nmotor;              /* number of motors */
  117.  
  118. /***********************************************************************
  119.                      T I M E    S I M U L A T I O N
  120. ***********************************************************************/
  121.  
  122. #ifndef RTIME
  123.  
  124. /*---------------------------------------------------------------------
  125. PROCEDURE
  126.     TSTEP  -  advance time forward one step
  127.  
  128. SYNOPSIS
  129.     void tstep(void)
  130.  
  131. LAST UPDATE
  132.     10 March 1988
  133.         check if simulation is initialized
  134. ---------------------------------------------------------------------*/
  135.  
  136. void tstep()
  137. {
  138.  
  139.     if (initflag)
  140.     {
  141.         simul();        /* compute one time step in the differential */
  142.                         /*   equation for the motor system.      */
  143.  
  144.         t += dt;        /* increment time by dt */
  145.  
  146.         runtimer();     /* simulate timer run */
  147.     }
  148.     else
  149.     {
  150.         printf("simulation module not initialized\n");
  151.  
  152.         tflag = 1;
  153.     }
  154.  
  155. }
  156.  
  157.  
  158.  
  159. /*----------------------------------------------------------------------
  160. PROCEDURE
  161.     SETTIMER  -  set countdown timer
  162.  
  163. SYNOPSIS
  164.     void SetTimer(ms)
  165.     long ms;
  166.  
  167. PARAMETER
  168.     period -  count down period in milliseconds
  169.  
  170. LAST UPDATE
  171.     15 January 1988
  172. ----------------------------------------------------------------------*/
  173.  
  174. void SetTimer(ms)
  175. long ms;
  176. {
  177.     
  178.     tflag = 0;                              /* timeout flag to false */
  179.  
  180.     ticks = ((ms * freq) + 500L) / 1000L;   /* 500 is for rounding */
  181.  
  182.     if (ticks <= 0L)
  183.         printf("simulation time step too coarse\n");
  184.  
  185. }
  186.  
  187.  
  188.  
  189. /*---------------------------------------------------------------------
  190. FUNCTION
  191.     TIMEUP  -  has timer timed out?
  192.  
  193. SYNOPSIS
  194.     int TimeUp()
  195.  
  196. RETURNS
  197.     1 if timer runs out, 0 otherwise
  198.  
  199. LAST UPDATE
  200.     1 March 1985
  201. ----------------------------------------------------------------------*/
  202.  
  203. int TimeUp()
  204. {
  205.  
  206.     return(tflag);
  207. }
  208.  
  209.  
  210.  
  211. /*----------------------------------------------------------------------
  212. PROCEDURE
  213.     RUNTIMER  -  simulates timer run
  214.  
  215. SYNOPSIS
  216.     static void runtimer()
  217.  
  218. LAST UPDATE
  219.     20 March 1988
  220.         one tick per simulation time step
  221. ----------------------------------------------------------------------*/
  222.  
  223. static void runtimer()
  224. {
  225.  
  226.     if (!tflag)             /* if timeout, just return, otherwise... */
  227.     {
  228.         --ticks;
  229.  
  230.         if (ticks <= 0L)    /* time has run out */
  231.             tflag = 1;
  232.     }
  233.  
  234. }
  235.  
  236. #endif  /* ifndef RTIME */
  237.  
  238. /***********************************************************************
  239.                         A / D    S I M U L A T I O N
  240.  
  241.     These routines dummies the routines in dash8.c which drives the
  242.     Metrabyte DASH8 A/D board.
  243.  
  244. ***********************************************************************/
  245.  
  246. void ad_init()
  247. {
  248.     int m;          /* motor number (also serves as loop index) */
  249.  
  250.  
  251.     for (m = 0; m < nmotor; m++)
  252.         (pmsim0 + m)->speed = 0.0;              /* motor is at rest */
  253.  
  254. }
  255.  
  256.  
  257. void ad_start(channel)
  258. int channel;
  259. {
  260.     /* nothing */
  261. }
  262.  
  263.  
  264. int ad_read(channel)
  265. int channel;
  266. {
  267.     int rval;                   /* return value */
  268.  
  269.  
  270.     if ((channel >= 0) && (channel < nmotor))
  271.     {
  272.         rval = (int)((pmsim0 + channel)->speed * VEL_SCALE);
  273.     }
  274.     else
  275.     {
  276.         rval = 0;
  277.     }
  278.  
  279.     if (rval > ADMAX)               /* apply converter limits */
  280.         rval = ADMAX;
  281.     else if (rval < ADMIN)
  282.         rval = ADMIN;
  283.  
  284.     return(rval);
  285. }
  286.  
  287.  
  288.  
  289. int ad_wread(channel)
  290. int channel;
  291. {
  292.  
  293.     return(ad_read(channel));
  294. }
  295.  
  296.  
  297. /***********************************************************************
  298.                      D / A    S I M U L A T I O N
  299.  
  300.     These routines dummies the routines in dac2.c which drives the
  301.     Metrabyte DAC02 D/A board.  See rtsim2.c also.
  302.  
  303. ***********************************************************************/
  304.  
  305. void da_init()
  306. {
  307.     int m;
  308.  
  309.  
  310.     for (m = 0; m < nmotor; m++)
  311.         (pmsim0 + m)->torque = 0.0;
  312.  
  313. }
  314.  
  315.  
  316. void da_limits(lo, hi)
  317. int *lo, *hi;
  318. {
  319.  
  320.     *lo = DAMIN;
  321.     *hi = DAMAX;
  322.  
  323. }
  324.  
  325.  
  326. int da_write(channel, value)
  327. int channel, value;
  328. {
  329.  
  330.     if ((channel >= 0) && (channel < nmotor))
  331.     {
  332.         if (value < 0)          /* OR with a binary number having 0's for */
  333.             value |= ~0xfff;    /*   the lower 12 bits and 1's elsewhere  */
  334.         else 
  335.             value &= 0xfff;     /* for positive values, just save the     */
  336.                                 /*   lower 12 bits.                       */
  337.  
  338.         (pmsim0 + channel)->torque = value / DASCALE;
  339.     }
  340.  
  341.     return(0);
  342. }
  343.  
  344.  
  345. /**********************************************************************
  346.                    P L A N T    S I M U L A T I O N
  347. ***********************************************************************/
  348.  
  349. /*----------------------------------------------------------------------
  350. PROCEDURE
  351.     SIM_RESET  -  reset simulation module
  352.  
  353. SYNOPSIS
  354.     void sim_reset(cline)
  355.     char *cline;
  356.  
  357. PARAMETER
  358.     cline  -  user's input command line
  359.  
  360. REMARKS
  361.  
  362. LAST UPDATE
  363.     20 March 1988
  364.         adapt timer resolution to simulation time step
  365. ----------------------------------------------------------------------*/
  366.  
  367. void sim_reset(cline)
  368. char *cline;
  369. {
  370.  
  371.     sscanf(cline, "%lf", &dt);          /* get the step size */
  372.  
  373. #ifdef DEBUG
  374.     printf("<init> dt = %f\n", dt);
  375. #endif
  376.  
  377.     t = 0.0;                            /* reset time */
  378.     freq = (long)(1.0 / dt  +  0.5);    /* adapt timer frequency */
  379.  
  380.     ad_init();      /* reset ADC (speed) */
  381.     da_init();      /* reset DAC (torque) */
  382.  
  383. }
  384.  
  385.  
  386.  
  387. /*----------------------------------------------------------------------
  388. PROCEDURE
  389.     DO_SIMUL  -  special hook into simul() for demo purposes
  390.  
  391. SYNOPSIS
  392.     void do_simul(void);
  393.  
  394. LAST UPDATE
  395.     10 April 1988
  396.         creation
  397. ----------------------------------------------------------------------*/
  398.  
  399. void do_simul()
  400. {
  401.     simul();
  402. }
  403.  
  404.  
  405. /*----------------------------------------------------------------------
  406. PROCEDURE
  407.     simul  -  simulates motor plant
  408.  
  409. SYNOPSIS
  410.     static void simul(void)
  411.  
  412. REMARKS
  413.     Solve the motor system differential equation.  This version uses 
  414.     the Euler method for integration; it isn't very efficient, but its 
  415.     easy to program!
  416.  
  417.     Simulates a motor with pure inertial load only.
  418.  
  419. LAST UPDATE
  420.     1 April 1988
  421.         add friction
  422. ----------------------------------------------------------------------*/
  423.  
  424. static void simul()
  425. {
  426.     MOTSIM *pms;        /* pointer to motor simulation descriptor */
  427.     int m;              /* loop index (motor number) */
  428.     double friction;    /* friction proportional to speed */
  429.  
  430.  
  431.     for (m = 0, pms = pmsim0; m < nmotor; m++, pms++)
  432.     {
  433.         friction = pms->speed * 0.1;    /* 0.1 factor is arbitrary */
  434.  
  435.         pms->speed += (pms->torque - friction) * dt / pms->inertia;
  436.     }
  437.  
  438. }
  439.  
  440.  
  441. /***********************************************************************
  442.             I N I T I A L I Z A T I O N    R O U T I N E
  443. ***********************************************************************/
  444.  
  445. /*----------------------------------------------------------------------
  446. PROCEDURE
  447.     SIM_INIT  -  initialize simulation module
  448.  
  449. SYNOPSIS
  450.     void sim_init(nm)
  451.     int nm;
  452.  
  453. PARAMETER
  454.     nm  -  number of motors
  455.  
  456. REMARKS
  457.     In a "real" real-time program, this function would be used to set 
  458.     up interrupts, set clock modes, etc.  In this simulation version,
  459.     it sets time to zero and sets the motor simulation initial conditions.
  460.  
  461. LAST UPDATE
  462.     20 March 1988
  463.         rearrange sequence
  464. ----------------------------------------------------------------------*/
  465.  
  466. void sim_init(nm)
  467. int nm;
  468. {
  469.     int m;              /* motor number (loop index also) */
  470.     MOTSIM *pms;        /* pointer to motor simulation block */
  471.  
  472.  
  473.     /* allocate memory for the motor simulation data structures */
  474.  
  475.     if ((pmsim0 = (MOTSIM *)calloc(nm, sizeof(MOTSIM))) != (MOTSIM *)NULL)
  476.     {
  477.         pms = pmsim0;       /* starting with first structure... */
  478.  
  479.         for (m = 0; m < nm; m++)
  480.         {
  481.             pms->inertia = 1.0;     /* initialize with defaults */
  482.             pms->speed = 0.0;
  483.             pms->torque = 0.0;
  484.  
  485.             pms++;                  /* next motor */
  486.         }
  487.  
  488.         nmotor = nm;                /* record number of motors */
  489.  
  490.         initflag = 1;
  491.     }
  492.     else
  493.     {
  494.         printf("INSUFFICIENT MEMORY: ");
  495.         printf("cannot allocate %d simulation structures\n", nm);
  496.  
  497.         exit(1);
  498.     }
  499.  
  500. }
  501.  
  502.