Liren Large Software Subsidy 15

home *** CD-ROM | disk | FTP | other *** search

/ Liren Large Software Subsidy 15 / 15.iso / s / s300 / 1.ddi / CHAP4 / RTSIM2.C < prev next >

Wrap

C/C++ Source or Header | 1988-05-02 | 11.3 KB | 433 lines

/********************************************************************** FILE rtsim1.c - d.c. speed control, simulation module 1 ENTRY ROUTINES tstep - advance one time step SetTimer - set count-down timer TimeUp - has count down completed? ad_init - simulates A/D initialization ad_start - simulates starting an A/D read ad_read - simulates reading A/D value ad_wread - simulates read from A/D da_init - simulates D/A initialization da_limits - returns simulation D/A output limits da_write - simulates write to D/A PRIVATE ROUTINE simul - motor plant simulation INITIALIZATION ROUTINE sim_init - initialize simulation REMARKS This module simulates the interface and motor hardware and the passage of "real" time. This module must thus provide the same interface functions as the modules controlling the hardware. To use "real" time, compile this module with the /DFTIME switch; this will cause tstep() to use ftime() calls to tell time. LAST UPDATE 1 December 1987 restructure and modify for MSC 4.00 and 5.00 12 March 1988 modify for cascade1.c & include declarations to satify /W3 Copyright(C) 1984-1987 D.M. Auslander and C.H. Tham **********************************************************************/ /*********************************************************************** I M P O R T S ***********************************************************************/ #include <stdio.h> #include "envir.h" /* environment specifications */ #include "dash8.h" /* declarations for dash8.c */ #include "dac2.h" /* declarations for dac2.c */ #include "time0.h" /* declarations for time0.c */ /*********************************************************************** F O R W A R D D E C L A R A T I O N S ***********************************************************************/ #ifdef ANSI extern void tstep(void); /* these are referened outside of */ extern void sim_init(char *); /* this simulation module. */ static void simul(void); /* these are not referenced outside */ /* of this simulation module. */ static void runtimer(void); #else extern void tstep(); extern void sim_init(); void simul(); void runtimer(); #endif /********************************************************************** P R I V A T E D A T A **********************************************************************/ #define VEL_SCALE 512.0 /* scale speed to A/D units */ #define POS_SCALE 512.0 /* scale position to A/D units */ #define ADMAX 2047 /* max A/D input value */ #define ADMIN -2048 /* min A/D input value */ #define DASCALE 2047.0 /* scale D/A value to torque */ #define DAMIN -2048 /* min. output value accepted */ #define DAMAX 2047 /* max. output value accepted */ static double inertia = 1.0; /* motor system inertia */ static double speed; /* simulated motor velocity */ static double position; /* simulated motor position */ static double t; /* time */ static double dt; /* simulation step size in seconds */ static double torque = 0.0; /* motor torque */ static long freq; /* timer frequency (ticks/sec) */ static long ticks; /* how many ticks to timeout */ static int tflag; /* set to 1 if timer runs out */ static int initflag = 0; /* indicates if module initialized */ /*********************************************************************** T I M E S I M U L A T I O N ***********************************************************************/ /*--------------------------------------------------------------------- PROCEDURE TSTEP - advance time forward one step SYNOPSIS void tstep() LAST UPDATE 10 March 1988 check if simulation is initialized ---------------------------------------------------------------------*/ void tstep() { if (initflag) { simul(); /* compute one time step in the differential */ /* equation for the motor system. */ t += dt; /* increment time by dt */ runtimer(); /* simulate timer run */ } else { printf("simulation module not initialized\n"); tflag = 1; } } /*---------------------------------------------------------------------- PROCEDURE SETTIMER - set countdown timer SYNOPSIS void SetTimer(ms) long ms; PARAMETER period - count down period in milliseconds LAST UPDATE 15 January 1988 ----------------------------------------------------------------------*/ void SetTimer(ms) long ms; { tflag = 0; /* timeout flag to false */ ticks = ((ms * freq) + 500L) / 1000L; /* 500 is for rounding */ if (ticks <= 0L) printf("simulation time step too coarse\n"); } /*--------------------------------------------------------------------- FUNCTION TIMEUP - has timer timed out? SYNOPSIS int TimeUp() RETURNS 1 if timer runs out, 0 otherwise LAST UPDATE 1 March 1985 ----------------------------------------------------------------------*/ int TimeUp() { return(tflag); } /*---------------------------------------------------------------------- PROCEDURE RUNTIMER - simulates timer run SYNOPSIS static void runtimer() LAST UPDATE 20 March 1988 one tick per simulation time step ----------------------------------------------------------------------*/ static void runtimer() { if (!tflag) /* if timeout, just return, otherwise... */ { --ticks; if (ticks <= 0L) /* time has run out */ tflag = 1; } } /*********************************************************************** A / D S I M U L A T I O N These routines dummies the routines in dash8.c which drives the Metrabyte DASH8 A/D board. Channel 0 reads speed and channel 1 reads position for cascade1.c ***********************************************************************/ void ad_init() { speed = 0.0; position = 0.0; } void ad_start(channel) int channel; { /* nothing */ } int ad_read(channel) int channel; { int rval; /* return value */ if (channel == 0) { rval = (int)(VEL_SCALE * speed); /* scale and convert to int type */ } else if (channel == 1) { rval = (int)(POS_SCALE * position); } else rval = 0; if (rval > ADMAX) /* apply converter limits */ rval = ADMAX; else if (rval < ADMIN) rval = ADMIN; return(rval); } int ad_wread(channel) int channel; { return(ad_read(channel)); } /*********************************************************************** D / A S I M U L A T I O N These routines dummies the routines in dac2.c which drives the Metrabyte DAC02 D/A board. The channel parameter is ignored. The DAC2 is a 12-bit digital-to-analog converter configured for bipolar output. Thus, the output parameter values range from -2048 to 2047. Only lower 12 bits will be used, treating those 12 bits as a 2's complement signed number. Thus if the output value is negative, the lower 12 bits will be preserved and all higher order bits will be converted to 1's. Note the use of ~0xfff instead of 0xf000, this makes the program more portable across machines with different integer sizes. ***********************************************************************/ void da_init() { torque = 0.0; } void da_limits(lo, hi) int *lo, *hi; { *lo = DAMIN; *hi = DAMAX; } int da_write(channel, value) int channel, value; { if (value < 0) /* OR with a binary number having 0's for */ value |= ~0xfff; /* the lower 12 bits and 1's elsewhere */ else value &= 0xfff; /* for positive values, just save the */ /* lower 12 bits. */ torque = value / DASCALE; /* rescale for motor drive torque. */ return(0); /* indicate all is well */ } /********************************************************************** P R I V A T E R O U T I N E S The following routines are declared "static". These routines can be directly accessed only by routines inside this file; they are not "visible" to routines outside this file. Hence they are private to this file. Private routines enhances data security and helps avoid unexpected name conflicts in large programming projects. ***********************************************************************/ /*---------------------------------------------------------------------- PROCEDURE SIMUL - simulates motor plant SYNOPSIS static void simul() REMARKS Solve the motor system differential equation. This version uses the Euler method for integration; it isn't very efficient, but its easy to program! LAST UPDATE 12 January 1988 add friction term to make it more interesting 12 March 1988 add position using trapeziod integration ----------------------------------------------------------------------*/ static void simul() { double friction; /* friction term */ double pspeed; /* previous speed */ pspeed = speed; friction = speed * 0.1; /* this is quite arbitrary */ speed += (torque - friction) * dt / inertia; position += 0.5 * (pspeed + speed) * dt; } /*********************************************************************** I N I T I A L I Z A T I O N R O U T I N E ***********************************************************************/ /*---------------------------------------------------------------------- PROCEDURE SIM_INIT - system initialization SYNOPSIS void sim_init(cline) char *cline; PARAMETER cline - user's input command line REMARKS In a "real" real-time program, this function would be used to set up interrupts, set clock modes, etc. In this simulation version, it sets time to zero and sets the motor simulation initial conditions. LAST UPDATE 20 March 1988 adapt timer resolution to simulation time step ----------------------------------------------------------------------*/ void sim_init(cline) char *cline; { sscanf(cline, "%lf", &dt); /* get the step size. */ #ifdef DEBUG printf("<init> dt = %lf\n", dt); #endif t = 0.0; /* reset time */ freq = (long)(1.0 / dt + 0.5); /* adapt timer frequency */ speed = 0.0; /* motor is initially at rest */ position = 0.0; torque = 0.0; initflag = 1; /* simulation module is now initialized */ }