MOTOROLA Order this document by: A N 1 2 1 3 / D SEMICONDUCTOR APPLICATION NOTE 16-Bit DSP Servo Control With the MC68HC16Z1 By David Wilson INTRODUCTION This application note discusses digital filter implementation of Proportional, Integral, Differential (PID) control algorithms. The implementation takes advantage of the control-oriented digital signal processing capabilities of the Motorola M68HC16 family of modular microcontrollers. Microcontrollers have come a long way in the past two decades. Once relegated strictly to computer applications, these devices have steadily encroached on domains previously dominated by analog technology. Closed-loop control systems are among the most recent bastions to fall. Control systems based on digital processing of measured values are inherently less sensitive to changes in temperature and to aging than systems implemented with analog circuitry. In addition, digital system performance can be changed by developing new software rather than by physically altering a PC board. In fact, many emerging controller technologies, such as adaptive control, would be economically unattainable if not for digital processing capabilities. MICROCONTROLLERS AND DIGITAL SIGNAL PROCESSORS After the decision to use a digital solution has been made, a designer must evaluate system requirements to determine what type of device is best suited for the job. The decision often comes down to a choice between a standard microcontroller or a digital signal processor. Although design constraints vary, all digital signal processors are designed specifically to perform algebraic sum of products calculations at high speeds. Standard microcontrollers are best suited for applications that require relatively little real-time control, and also require the controller to perform other tasks, such as running an operating system or user interface. Digital signal processors, on the other hand, are generally used when a control algorithm is real-time intensive, and other system tasks can be handled by a master processor. There is thus a price-performance gap between general-purpose controllers and specialized, dedicated DSP engines. THE M68HC16 FAMILY The M68HC16 family of modular microcontrollers bridges the gap between standard microcontrollers and digital signal processors. The CPU16 module provides a rich instruction set as well as dedicated controloriented DSP capability, while other system modules provide a variety of interfacing options. The high level of functional integration in M68HC16 devices reduces the amount of external hardware necessary to achieve a complete system solution. The M68HC16 family bridges another gap by providing a migration path from the M68HC11 family of 8-bit controllers to the M68300 family of 32-bit modular controllers. Many M68HC16 and M68300 modules, such as the general-purpose timer (GPT), queued serial module (QSM), and system integration module (SIM), are identical. Use of the SIM provides both families with a common external bus interface. (c) MOTOROLA INC, 1996 The CPU16 programming model is similar to that of the M68HC11 CPU, and the CPU16 instruction set is upwardly code-compatible with that of the M68HC11 CPU. However, the CPU16 also provides significant additional capabilities. With dedicated multiply and accumulate registers and 18 instructions added specifically for DSP support, M68HC16 family devices are general-purpose microcontrollers capable of performing DSP operations, not just DSP engines with a few additional embedded-control features. This is an important distinction because most digital signal processors do not have bit manipulation capability, multiple interrupt vectors, or a flexible software stack. Although M68HC16 devices do not perform multiply and accumulate operations as rapidly as some dedicated digital signal processors, they are ideally suited for applications such as motion control systems. PID CONTROLLER BASICS PID controllers offer some distinct advantages over other control topologies; but nothing is free --as in all design processes, there must be trade-offs. Figure 1 is a block diagram of an analog PID control structure. P INPUT E (S) 1 S I S D OUTPUT C (S) AN1215 PID CONTROLLER BLOCK Figure 1 PID Controller Block Diagram PID TRANSFER FUNCTION The transfer function (as a function of s) is : 2 C(s) Ps + I + Ds ----------- = -------------------------------S E(s) where: (EQ 1) C(s) is the output of the PID section E(s) is the input to the PID section (usually servo error) P is the multiplier for the servo error I is the multiplier for the integral of the servo error D is the multiplier for the derivative of the servo error s is the Laplace complex frequency variable. The previous equation shows that the PID controller has a pole at s = 0, and two zeros : 2 - P P - 4DI s = --------------------------------------2D (EQ 2) The two zeros are real-valued when 4DI P2. A Bode plot of the PID transfer function with real-valued zeros reveals that one of the zeros is used to brake the 20 dB/decade descent associated with the integrator, and the other is used to provide a 20 dB/decade rise and positive phase lead required to stabilize the system. MOTOROLA 2 AN1213/D TRANSFER FUNCTION TERMS Each of the transfer function terms affects system performance. The P Term The Proportional term is the most subtle and perhaps most misunderstood term in the PID algorithm. The P term amplifies the error signal by a constant amount. However, P is not in series, but in parallel with I and D, which implies that P cannot be used to scale the transfer function amplitude at all frequencies. Instead, the P term interacts with the I and D terms to determine the placement of the zeros in the controller openloop transfer function. Figure 2 shows a root locus solution to the numerator of Equation 1 as P is varied with respect to I and D. j P= (I / D) (I / D) P=0 P= P = (4DI) S PLANE -j (I / D) P=0 AN1213 DELTA P ON ZEROS Figure 2 Effect of Varying P on Zeros The I Term The Integral term gives the servo loop that inflexible, stubborn feel. Since the I term adjusts the amount of integrated error mixed to the output of the filter, any I value other than zero implies that no steady state error can be tolerated by the servo loop. In other words, given sufficient time, a PID control loop will eventually servo the output to the exact value of the commanded input. In the frequency domain, the I term also affects placement of the zeros, as shown in Figure 3. For I = 0, one of the zeros is at s = -P/D, and the other zero is at s = 0, which means that it will cancel the integrator pole at s = 0. This makes sense intuitively since the integrator is turned off if I equals zero. As I increases, the servo loop becomes "snappier", i.e., it responds more quickly to steady state error. It appears that adding an integrator to the servo loop would be a panacea for motion control headaches. However, while adding an integrator does address steady state error, it can also have a negative impact on system dynamics. The effect is most easily seen in the time domain. Consider a linear PID system that performs servo control. Initially, the controlled motor is at rest, with zero position error. Torque is applied to the motor shaft, changing its position and holding it in the new position. The control system senses a steadystate error and tries to return the shaft to the commanded position. Since the example system is linear, control voltage continues to increase as a result of integrated error. While the increasing control voltage could cause the motor to overheat, this is not the only detrimental effect. If the applied torque is suddenly removed while integrator output is large, the motor shaft will spin past the desired shaft position while control voltage is "dumped". Eventually, a zero steady-state condition is achieved, but in an underdamped (and potentially unacceptable) manner. Because this situation is similar to winding up a spring and then letting it go, the term "wind-up" is used to describe it. Techniques to mitigate wind-up are discussed later in this note. AN1213/D MOTOROLA 3 I= 2 I= P 4D I=0 -P D I=0 -P 2D I= AN1213 DELTA I ON ZEROS Figure 3 Effect of Varying I on Zeros The D Term The Derivative term has its greatest effect on servo-loop damping and stability. As Figure 4 shows, increasing the value of D from 0 to P2/4I causes both zeros to move toward -2I/P. As this happens, the higherfrequency zero takes on a value that can provide useful phase lead to offset the phase lag introduced by poles elsewhere in the system. The design of the derivative portion of a PID controller is critical to system performance. In a position servo, the feedback position signal is differentiated (either directly or indirectly) to create a signal proportional to the output velocity. In systems that use a digital feedback mechanism (such as shaft encoders), velocity information is also quantized, typically in encoder tics per sampling interval. At low velocities, the effect of quantization on system performance is pronounced because each quantization step represents a large portion of velocity signal amplitude. This can cause an audible scraping noise or unnecessary motor heating at low speeds. A velocity state observer can be used to mitigate low-speed quantization effects. The state observer uses a software model of the load to synthesize a higher-resolution velocity signal. Each sample period, PID controller output is input to the model, and the model generates an estimate of output position. The estimate is compared to actual encoder position to generate an error value, which is used to refine the estimate for the next sample period. A simpler way to deal with this problem is to calculate the velocity information at a lower sampling frequency, thus increasing the number of encoder tics per sampling interval for a given velocity. A similar technique is discussed in the next section of this note. The location of the differentiator in the feedback loop also affects performance. In Figure 1, the differentiator input is the error signal. Since the commanded input position signal is a component of the error signal, any abrupt change in commanded position is differentiated as if it were feedback position, resulting in a "popping" effect at the filter output. An alternate topology can provide more satisfactory performance, as shown in the next section. MOTOROLA 4 AN1213/D D=0 2 D= P 4I D=0 -2I P D= -I P AN1213 DELTA D ON ZEROS Figure 4 Effect of Varying D on Zeros DESIGNING A PID FILTER PID TOPOLOGY To implement a PID control algorithm on any processor, methods of computing the functions specified by the controller (an integral and a derivative) must be developed. Once these methods are established, the digital PID controller transfer function is calculated in much the same way as the analog version. Unfortunately, because this is a sampled system, the Laplace transform or the s domain cannot be directly used, as in analog PID calculations. To address this problem, a separate frequency space called the "z" domain has been developed just for sampled systems. Using the z domain, sampled approximations of many common functions can be represented using the variable "z" just as "s" is used to represent linear analog functions. For the sake of simplicity, assume the following definitions are true: Where: Tz INTEGRATOR = ------------z-1 (EQ 3) z is the complex sampled frequency variable T is the sampling frequency period, in seconds. This form is derived from a step-invariant analysis --the filter is constructed by dividing the Z transform of a specified input (a step function) into the Z transform of the desired output for that input (a ramp function). The differentiator is simply the inverse, or: z-1 DIFFERENTIATOR = ------------Tz (EQ 4) Although these are not the only z-domain representations of these functions, they are widely used in control applications. See Reference 1 for more information on this topic. AN1213/D MOTOROLA 5 To mitigate encoder velocity quantization noise, the derivative function is followed by an "n-point averager", which averages velocity information over a range of samples to provide finer resolution. However, this crude low-pass filter also introduces phase lag proportional to n that counteracts the desired phase lead generated by the differentiator. To balance these two constraints, n is set equal to two, which effectively doubles encoder resolution per sampling interval. The derivative stage transfer function is : z-1 1 1 VELOCITY = ------------- --- + ------ Tz 2 2z (EQ 5) Figure 5 shows the PID controller and transfer functions of parasitic effects found in the system. All items in Figure 5 except the power stage, the motor, and the encoder, are implemented by the M68HC16 device. DSP ACCUMULATOR P COMMANDED POSITION OUT POSITION ERROR CALCULATION DELAY PWM LIMIT OUT EXP(- STC ) IN INTEGERATOR LIMIT IN I Tz z-1 PROFILER THRESHOLD SWITCH S z-1 Tz K1 VOLTS PWM G (S) RADIANS VOLT 2 POINT AVERAGER INTEGRATOR ON / OFF I -D MOTOR 4 POINT AVERAGER SAMPLE AND HOLD OUTPUT POSITION DIFFERENTIATOR z-1 Tz ENCODER POSITION K2 COUNTS RADIAN AN1213 PID SYSTEM BLOCK Figure 5 System Block Diagram The differentiator input is the encoder signal, not the error signal, for reasons discussed in the previous section. The commanded input signal is not differentiated, and the stability of the system is not affected because the overall open-loop transfer function remains the same. The integrator has several associated features that improve system damping. One is a software switch to enable the integrator only when it is needed. Since this is a position servo, it is assumed that the integrator is not required when the velocity magnitude increases above a specified threshold. This prevents an error signal from being integrated over the entire duration of a change in position, which would require overshoot to dump the error. Figure 6, which shows system response to a step function, illustrates the effectiveness of this technique. MOTOROLA 6 AN1213/D WITHOUT INTEGRATOR SWITCHING WITH INTEGRATOR SWITCHING AN1213 PWM STEP RESPONSE (INT) Figure 6 Step Response of System With and Without Integrator Switching Integrator output magnitude is limited to mitigate the wind-up effect associated with PID integrators. The limit is application-dependent, and should be set to the minimum value required to generate an output sufficient to overcome any anticipated load resistance. In theory, the sampling process is modeled as a series of impulse functions, which implies a PID filter calculation time of zero. In reality, the amount of time required to execute a digital filter algorithm must be accounted for, since it introduces phase lag into the system. Even though the lag is minuscule at the frequencies of interest in this note, it is included for the sake of completeness, and is given by : G cd ( 2 ) = e - sTc (EQ 7) Where Tc = Calculation time in seconds The MC68HC16Z1 GPT module has two PWM ports, one of which is used as an output for the digital filter. The PWM value is updated every sample period, and is latched until the following sample period. This sample and hold function introduces phase lag, which must be considered. There are several mathematical models for a sample and hold; the one selected for this application is : 1 z-1 G SH ( s, z ) = --- ------------- s Tz (EQ 8) The commanded position input to the digital filter comes from another software routine called a profiler, which is responsible for generating a series of positions corresponding to a specific velocity profile (a trapezoidal profile is used for this application). Profiler design is beyond the scope of this note, but, because profiler execution time rivals that of the digital filter, a new commanded position is not calculated at each digital filter sampling interval (488 s). While the CPU16 could easily perform the extra calculations, a recalculation interval equal to four times the sampling interval was judged sufficient. However, lowering the profiler update rate creates another problem. The commanded position input now has a large step change AN1213/D MOTOROLA 7 every fourth filter sample. This introduces a frequency component corresponding to profiler update rate at the filter output, which is quite audible in the motor windings. To compensate, profiler outputs are shifted through a 4-point averager running at the same frequency as the PID filter. The averager linearly interpolates or smoothes profiler data at a 4X oversampling rate, which in turn eliminates motor noise. Filter output waveforms shown in Figure 7 illustrate the effectiveness of this technique. WITHOUT PROFILER DATA FILTERING WITH PROFILER DATA FILTERING AN1213 PWM OUTPUT RIPPLE Figure 7 PWM Output Ripple With and Without Profiler Filtration SELECTING PID COEFFICIENTS The following points should be taken into consideration when designing a servo. To assure robust operation and speed, it is generally desirable to have as high a system frequency response as possible. To obtain adequate damping performance, phase margin (180 degrees minus the phase lag of the open-loop transfer function evaluated at unity gain) should be as large as possible. To make certain the system can tolerate a large change in gain (e.g., sagging power supply voltage or drifting load parameters) without loss of stability, gain margin (difference in gain between the open-loop transfer function gain evaluated at the point of -180 degrees phase shift, and unity gain) should be as great as possible. All of these conditions can be observed by generating magnitude and phase frequency plots of the openloop transfer function G(s,z)H(s,z). The open-loop function can be calculated by breaking the servo loop diagram at a convenient point, then multiplying all individual transfer functions encountered around the loop until arriving back at the break point. However, this procedure yields an equation that is a function of both s and z. Fortunately, there is a mathematical relationship between z and s that allows representation of the equation as a function of s alone. MOTOROLA 8 AN1213/D z = e sT (EQ 9) Where T is the sampling period in seconds Assume that the PID controller section is bypassed and calculation delay is zero. By using equation 10 as the transfer function for the motor, and assigning K1 and K2 (Figure 5) values of 0.1875 and 636.62, an open-loop transfer equation as a function of s is obtained. G MOTOR Where: 1 ------Ke ( s ) = ------------------------------------------------s ( 1 + st m ) ( 1 + st e ) (EQ 10) Ke is the back EMF constant (70.61 mV/(rad/second)) tm is the mechanical time constant (6.2 ms) te is the electrical time constant (1.62 ms) Open loop unity gain frequency is approximately 74 Hz, but phase margin is about -20 degrees. The system will oscillate due to poor phase margin --phase compensation is needed. Figure 8 and Figure 9 are openloop transfer function magnitude and phase plots. 54 48 42 AMPLITUDE (dB) 36 30 24 18 12 0 -6 -12 2 3 4 5 6 7 8 9 10 20 FREQUENCY (Hz) 30 40 50 100 200 AN1213 PWM OPEN LOOP MAG Figure 8 Open-Loop Magnitude Without Compensation AN1213/D MOTOROLA 9 200 160 PHASE MARGIN = -19.8 120 PHASE (DEGREES) 80 40 0 -40 -80 -120 -160 -200 2 3 4 5 6 7 8 9 10 20 30 40 50 FREQUENCY (Hz) 100 200 AN1213 PWM OPEN LOOP PHASE Figure 9 Open-Loop Phase Without Compensation Values of P, I, and D are iteratively selected using a computer model of the open-loop transfer function that includes the PID controller. An initial Tc value of 30 s is assumed. Phase margin, gain margin, and frequency response for several sets of terms are calculated, and working values of P = 0.16, I = 5, and D = 1E-3 are selected. Gain margin is -14.4 dB, and phase margin is improved to about 60 degrees, at the expense of lowering open-loop frequency response to 41 Hz. At this frequency, the phase lag generated by the 2point velocity signal averager is 3.6 degrees. Were a 4-point averager used, phase lag would increase to 10.8 degrees at 41 Hz. Figure 10 and Figure 11 are open-loop magnitude and phase plots generated from the working values. Figure 12 and Figure 13 are controller transfer function plots generated from the working values. Low-frequency amplification caused by the integrator and high-frequency amplification caused by the differentiator are apparent. Figure 13 shows the positive phase generated by the PID controller. MOTOROLA 10 AN1213/D 36 30 24 PHASE (DEGREES) 18 12 6 0 GAIN MARGIN = -14.4 dB -6 146 Hz -12 -18 -24 2 3 4 5 6 7 8 9 10 20 30 40 50 100 FREQUENCY (Hz) 200 AN1213 PWM OPEN LOOP MAG (COMP) Figure 10 Open-Loop Magnitude With Compensation 200 160 120 PHASE (DEGREES) 80 40 0 -40 -80 -120 PHASE MARGIN = 59.3 -160 -200 2 3 4 5 6 7 8 9 10 20 30 41 Hz 40 50 FREQUENCY (Hz) 100 200 AN1213 PWM OPEN LOOP PHASE (COMP) Figure 11 Open-Loop Phase With Compensation AN1213/D MOTOROLA 11 36 30 24 AMPLITUDE (dB) 18 12 6 0 -6 -12 -18 -24 2 3 4 5 6 7 8 9 10 20 30 40 50 100 FREQUENCY (Hz) 200 AN1213 PWM PID MAG Figure 12 PID Controller Magnitude 60 48 36 PHASE (DEGREES) 24 12 0 -12 -24 -36 -48 -60 2 3 4 5 6 7 8 9 10 20 30 40 FREQUENCY (Hz) 50 100 200 AN1213 PWM PID PHASE Figure 13 PID Controller Phase MOTOROLA 12 AN1213/D SAMPLED TIME DOMAIN SOLUTION To realize the controller as a difference equation, solve for the output of each portion of the PID controller. The P Term Since P is a constant multiplier, the solution is straightforward : P(z) ----------- = P or P(z) = PE(z) E(z) where: (EQ 11) P(z) is the output of the proportional stage of the PID controller E(z) is the error between the commanded position and the feedback position Performing the transform to the sampled time domain : P(n) = PE(n) (EQ 12) The notation "n" is used to represent the present sample. Similarly, n+1 indicates the next or future sample, n-1 indicates the previous sample. The I Term From Equation 3, we obtain: Iz ITz ----------- = -----------E(z) z-1 Where : (EQ 13) I(z) is the output of the integrator stage T is the sample period (488 s). This results in: zI(z) - I(z) = ITz E(z) (EQ 14) Next, the "Shift of Sequence" theorem of the Z transform is used to obtain a solution in the sampled time domain. The theorem is stated as follows: a If Z[x(n)] = X(z), Then Z[x(n+a)] = z X(z) (EQ 15) Where: Z indicates the Z transform operation. Applying this theorem in reverse yields: I ( n + 1 ) = I(n) + TIE ( n + 1 ) (EQ 16) Now shift the function in time (i.e., n +1 = n) to obtain : I(n) = I ( n - 1 ) + TIE ( n ) (EQ 17) Simply stated, "the present sample of the integrator output is equal to the previous sample of the integrator output plus T times I times the present sample of the error signal." Since output feedback is employed to calculate the next output value, this portion of the filter is classified as an IIR (infinite impulse response) filter. AN1213/D MOTOROLA 13 The D Term From Equation 5, derive the transfer function for the derivative term: D(z) D (z-1) 1 1 ----------- = ------------------- --- + ------ Tz 2 2z X(z) where: (EQ 18) X(z) is the feedback position signal D(z) is the output of the derivative stage. This equation can be reduced to : 2 2 2Tz D(z) = ( Dz - D )X(z) (EQ 19) Performing the transform to the sampled time domain yields : 2TD(n+2) = DX ( n + 2 ) - DX ( n ) (EQ 20) or D D ( n + 2 ) = ------- ( X ( n + 2 ) - X ( n ) ) 2T Shifting in time yields : D D ( n ) = ------- ( X ( n ) - X ( n - 2 ) ) 2T (EQ 21) In other words, the velocity information is derived from X(n), the present position feedback sample, and from X(n-2), the feedback sample made two periods earlier. This yields better quantization results at low speeds. COMBINING TERMS From Figure 5, we see that : Y(n) = P(n) + I(n) - D(n) (EQ 22) Where Y(n) is the PID controller output. Therefore, combining equations 12, 17, and 21 (EQ 23): D Y(n) = PE(n) + I(n-1) + TIE(n) - ------- ( X ( n ) - X ( n - 2 ) ) 2T P Term I Term D Term Since the proportional term and the integral term both operate directly on E(n), it might appear that these terms could be combined in Equation 23. However, as Figure 5 shows, the integrator must be gated with the velocity term and integrator output must be limited to mitigate wind-up. For these reasons, the P and I terms are kept separate. Since the terms P, I, D, and T are constants, equation 23 can be rewritten : Y(n) = PE(n) + I ( n - 1 ) + aE(n) + b ( X(n) - X ( n - 2 ) ) (EQ 24) where: P = 0.16 a = 0.00244 b = -1.0246 MOTOROLA 14 AN1213/D M68HC16 IMPLEMENTATION CODING THE FILTER The CPU16 can perform signed and unsigned 16-bit integer multiplication as well as signed and unsigned 16-bit fractional multiplication. Since the PID coefficients are neither all-integer nor all-fraction, the values must be scaled before calculations can be performed, and the same scaling must also be used to correct the filter output. Because CPU16 DSP instructions use fractional notation, each coefficient is divided by 2, so that: P' = 0.08 or $0A3D a' = 0.00122 or $0028 b' = -0.5123 or $BE6D The filter is implemented in an interrupt service routine (ISR) which is called each time the programmable interrupt timer (PIT) in the SIM times out (every 488 s). The portion of the ISR associated with the PID implementation is listed at the end of this application note. The complete code can be downloaded from Motorola Freeware Data Systems. Modem connection at (512) 891-3733. Internet address (ftp): freeware@aus.sps.mot.com. World-wide web: http://www.freeware.aus.sps.mot.com. SERVO CONTROL HARDWARE The MC68HC16Z1EVB provides an excellent platform for this application. Use of CPU16 background debugging mode, the QSM serial communication interface, and the EVB 16-bit DAC are particularly helpful. Figure 14 is a diagram of the hardware used in the servo system. The logic to motor interface module (available from Motorola) contains a complementary H-bridge driver (MPM3002) that is used to provide up to 60 volts to a motor load. The board is operated in four-quadrant mode (inverted PWM) --the PWM signal that drives one diagonal transistor pair is an inverted version of the signal that drives the other pair. The PWM interface module prevents excessive heating due to shootthrough current by delaying each enabling PWM edge approximately 2 s. This provides a switching deadband between the time one leg is turned off and the other leg is turned on. The PWM interface module also uses the current mirror feedback from the MPM3002 to provide cycle-by-cycle current limiting. For further information about the logic to motor interface module, refer to Interfacing Microcomputers to Fractional Horsepower Motors (AN1300). Operating with four-quadrant PWM implies that the waveform generated by the MC68HC16Z1 must be bipolar (50% PWM corresponds to no voltage on the motor). As the sample ISR code shows, this is accomplished by adding a fixed offset to the digital filter output before it is put in the PWM register. Once the value is in the PWM register, the general-purpose timer (GPT) generates the required PWM signal without further CPU intervention. The 1000-slot encoder on the motor shaft is processed by the Hewlett-Packard HCTL-2016 quadrature decoder, which accumulates the encoder count on an internal 16-bit up/down counter. One of the 12 MC68HC16Z1 chip-select outputs is programmed to perform the address decoding necessary to access and read the HCTL-2016 counter data. 16-bit data is read on data bus pins DATA[15:8] as two sequential 8-bit values. The 16-bit data word is then used to synthesize a 32-bit absolute position variable, as shown in the beginning of the PID ISR listing. AN1213/D MOTOROLA 15 + 24V MC68HC16Z1 EVB PWM INTERFACE MODULE PWM ENABLE SCI INTERFACE CURRENT LIMIT CURRENT FEEDBACK LOGIC TO MOTOR INTERFACE MODULE AMPLIFIED PWM 16-BIT DAC TO OSCILLOSCOPE HCTL 2016 USER INTERFACE PWM ENCODER MOTO R ENCODER FEEDBACK AN1213 PID HARDWARE BLOCK Figure 14 Hardware Block Diagram A user can control the application via a terminal connected to the serial communications interface (SCI) in the queued serial module (QSM). The MC68HC16Z1 EVB provides a 25-pin D connector for this purpose. To initiate motor movement, issue a MOVE command, specifying the final position, the maximum profiler velocity, and the acceleration of the move. The SCI transmits and receives such commands with very little CPU intervention, even during a move. The MC68HC16 EVB also provides for installation of a Burr-Brown PCM56P 16-bit serial DAC. If the QSM Serial Peripheral Interface (SPI) is used to drive the DAC, 16-bit DAC updates can be provided at approximately 4 s intervals (SCLK frequency = 4.19 MHz). DAC output can be used to probe portions of the servo loop that are not readily available in analog form, such as shaft encoder position or integrator stage output. The oscilloscope plots in this note were generated by transmitting variable values to the DAC and updating them every pass through the sample ISR (once every 488 s). CONCLUSIONS No matter how thorough the analysis, a working design usually requires some adjustment. The designer must compare actual system function to theoretical expectations, then tune system coefficients to achieve optimum performance. The D term has a dramatic effect on system stability, and should be the first term adjusted when damping performance is less than satisfactory. Figure 15 shows how varying D affects system damping, and indicates that the original calculated value for D provides for quick settling time with little or no overshoot. As mentioned earlier, the I term is used to "servo out" steady-state position error. To demonstrate, a frictional load was applied to the motor shaft, and the system step response was measured with the integrator enabled and disabled. When disabled, the position error was measured to be between 65 -69 encoder counts (approximately 6 degrees). With I set to its default value of 5, the error did not exceed one encoder count (0.09 degrees). MOTOROLA 16 AN1213/D As Equation 5 shows, the PID calculation must be performed quickly, to minimize phase delay introduced into the loop. Calculation time can be measured by connecting an oscilloscope to an I/O pin and running a subroutine that drives the pin high just before encoder sampling, then drives it low it just after the PWM register is loaded with a new value. Measured in this way, calculation time is roughly 30 s, which translates into 0.44 degrees of phase lag at the open-loop unity gain frequency (41 Hz), a minimal contribution to total system phase lag at that frequency. Also, since less than 10 percent of CPU time is spent controlling axis motion, multiple axes or more sophisticated control algorithms, including state observers, notch filters, and adaptive control, could easily be implemented. The techniques in this application note allow a designer to customize a controller for an application, rather than be forced to work around pre-packaged servo controls. The capabilities of the M68HC16 family also enable designers to respond quickly to changes, and permit solution of real-time DSP control problems without loss of the user friendliness provided by a complete microcontroller system. 0.61 X 10 -3 1 X 10 -3 1.586 X 10 -3 AN1213 PWM STEP RESPONSE Figure 15 Effect of Varying D on System Damping REFERENCES 1. Kuo, B., Digital Control Systems, Holt, Rinehart and Winston, Inc., 1980 2. Oppenheim, A.V., Schafer, R.W., Digital Signal Processing, Prentice-Hall, Inc., 1975 AN1213/D MOTOROLA 17 PID INTERRUPT SERVICE ROUTINE LISTING 60 61 00062E 3908 FA19 meas. 62 63 000632 37F5 0900 64 000636 37FA 0004 65 00063A 37F0 0008 new one 66 00063E BB00 67 000640 2775 68 000642 B000 69 000644 3735 FFFF 70 71 1m 2m 3m 000648 37F1 0008 4m 00064C 3773 0006 72 000650 377A 0002 73 74 000654 2771 003C 75 1m 2m 3m 000658 37F0 0004 4m 00065C 3772 0002 76 000660 37FA 0014 x(n) 77 78 79 000664 2776 tive 80 000666 BC10 81 000668 37B1 8000 82 00066C 3733 0000 83 000670 BC18 84 000672 37B5 8000 85 000676 37FA 0014 86 00067A B00E 87 88 00067C 89 00067C 37B0 8000 90 000680 3732 0000 E(n) 91 000684 BD04 tive 92 000686 37B5 7FFF 93 00068A 37FA 0014 94 95 96 00068E 97 00068E 2771 0002 98 1m 2m 3m 000692 37F0 000C 4m 000696 3772 000A 99 00069A 37FA 0010 100 101 102 103 00069E 2771 0006 104 0006A2 2773 000A 105 0006A6 2771 0002 106 0006AA 2773 0006 107 108 MOTOROLA 18 * Here we go! bset ldd std subd bmi clre bra enc_neg lde PORTF0,#test_bit1 ;set test_bit1 output for timing enc Xn+2 Xn_1+2 ;read the encoder. ;update lower word of Xn ;subtract old lower word from enc_neg ;IF delta is positive ; sign extend to AccE ($0000) ;ELSE ; sign extend to AccE ($FFFF) ;ENDIF add_delta #$FFFF add_deltaADDMLONGXn_1 +*Adds a 32-bit value in memory at "location" to +*the concatenated value in D and E. addd Xn_1+2 adce Xn_1 ste Xn ;Xn now updated. +subd + sbce lded pcommand SUBMLONGXn +*Subtract a 32-bit variable in memory at "location" from +*the concatenated value of the D and E registers. Xn+2 Xn std En ;E(n) = commanded position - *E(n) must be limited to a 16-bit number tste ;check whether E(n) is negabge addd adce bge ldd std bra Epositive subd sbce Epositive #$8000 #$0000 E_ok #$8000 En E_ok ;IF E(n) is negative ;add $00008000 to E(n) ; IF result is negative ; E(n) = $8000 ;ENDIF ;ELSE #$8000 #$0000 ; subtract $00008000 from blt E_ok ; IF result is zero or posi- ldd std #$7FFF En ; En = $7FFF ; ENDIF ;ENDIF E_ok +subd +sbce lded Xn SUBMLONG Xn_2 +*Subtract a 32-bit variable in memory at "location" from +*the concatenated value of the D and E registers. Xn_2+2 Xn_2 std Xn1 ;x(n) = x(n) - x(n-2) *shift the sampled encoder data lded sted lded sted Xn_1 Xn_2 Xn Xn_1 * now perform the digital filter AN1213/D 109 110 0006AE 111 1m 2m 0006B2 3m 0006B4 4m 0006B6 5m 0006B8 112 0006B8 113 0006BC 114 115 0006BE 116 0006C0 117 0006C2 118 0006C6 119 120 0006C8 121 0006C8 122 map) 123 0006CC 124 1m 2m 3m 0006CE 4m 0006D2 125 0006D6 126 127 128 0006DA 129 0006DC 130 0006DE 131 0006E2 132 0006E4 133 0006E6 134 0006EA 135 0006EE 136 137 0006F0 138 0006F0 139 0006F4 I(n) 140 0006F8 tive 141 0006FA 142 0006FE 143 000702 144 000706 145 146 000708 147 000708 148 149 00070C 150 151 00070C 152 00070E 153 table) 154 000712 155 000714 p*E(n) 156 000716 (since 157 able map) 158 00071A 159 00071C p*E(n) AN1213/D 37F5 0010 37B0 0005 BD06 ldd Xn1 ABS +*Take the absolute value of the D register. +tstd +bge 1positive +negd +1positive: subd #$0005 blt Icalc ;IF ABS(Xn) >= $0005 27F5 2775 2773 0018 B040 clrd clre sted bra In_1 I_loaded lded a 27F6 BCFE 27F2 Icalc 2771 0012 3727 37F1 001A 3773 0018 2773 0018 BC10 FC00 3733 0008 BC20 27F5 3735 FFF8 2773 0018 B018 37B0 0000 3732 0008 ; I(n-1) = 0 (clear "I" term) ;ELSE ; a ==> AccE , E(n) ==> AccD ; (E(n) follows a in variable fmuls ADDMLONG In_1 +*Adds a 32-bit value in memory at "location" with +*the concatenated value in D and E. +addd In_1+2 +adce In_1 sted In_1 ; I(n) = I(n-1) + a*E(n) ; Now limit the integrator ; term to 19 bits. bge Ipositive ; IF I(n) is negative addd #$0000 adce #$0008 ; add $00080000 to I(n) bge get_I ; IF result is negative clrd lde #$FFF8 sted In_1 ; I(n) = $FFF80000 bra I_loaded ; ENDIF ; ELSE Ipositive subd #$0000 sbce #$0008 ; subtract $00080000 from BD0A blt get_I 37B5 FFFF 3735 0007 2773 0018 B000 ldd lde sted bra #$FFFF #$0007 In_1 I_loaded lded In_1 ; IF result is zero or posi- ; I(n) = $0007FFFF ; ENDIF ; ENDIF get_I 2771 0018 I_loaded ;ENDIF 27B1 2771 0014 tedm lded 3727 3723 fmuls aced 2771 000E lded En ;AM = I(n-1) + a*E(n) ;E(n) ==> AccE , p ==> AccD ; (p follows E(n) in variable ;AM = I(n-1) + a*E(n) + b ;b ==> AccE, X(n) ==> AccD ;X(n) follows "b" in vari- 3727 3723 fmuls aced ;AM = I(n-1) + a*E(n) + MOTOROLA 19 160 ; + b*x(n) 161 162 00071E 27B6 aslm ;compensate for data scaling of 163 ;PID coefficients (multiply by 2). 164 000720 27B4 tmer ;transfer result to AccE rounded. 165 000722 377A 001C ste Yn ;Yn is output of filter 166 167 *Now the pwm value must be limited to an 8-bit value. 168 169 000726 27FB ted 170 000728 BC0A bge Ypositive ;IF Y(n) is negative 171 00072A FC7F addd #$007F ; add $007F to Y(n) 172 00072C BC14 bge get_Y ; IF result is negative 173 00072E 37B5 FF81 ldd #$FF81 174 000732 37FA 001C std Yn ; Y(n) = $FF81 (minimum value-- 175 ; PWM interface module always 176 ; needs a PWM edge to do cycle177 ; by-cycle current limiting) 178 179 000736 B00E bra Y_loaded ; ENDIF 180 ;ELSE 181 000738 Ypositive 182 000738 37B0 0080 subd #$0080 ; subtract $0080 from Y(n) 183 00073C BD04 blt get_Y ; IF result is zero or positive 184 00073E 37B5 007F ldd #$007F 185 000742 37FA 001C std Yn ; Y(n) = $007F (maximum PWM allowed) 186 ; ENDIF 187 ;ENDIF 188 000746 get_Y 189 000746 37F5 001C ldd Yn 190 00074A 37B1 0080 Y_loaded addd #$0080 191 00074E 17FA F926 stab PWMA ;scale PWM so that 50% is zero volts. 192 193 * We are done with PID filter at this point. Motorola reserves the right to make changes without further notice to any products herein. Motorola makes no warranty, representation or guarantee regarding the suitability of its products for any particular purpose, nor does Motorola assume any liability arising out of the application or use of any product or circuit, and specifically disclaims any and all liability, including without limitation consequential or incidental damages. "Typical" parameters can and do vary in different applications. All operating parameters, including "Typicals" must be validated for each customer application by customer's technical experts. Motorola does not convey any license under its patent rights nor the rights of others. Motorola products are not designed, intended, or authorized for use as components in systems intended for surgical implant into the body, or other applications intended to support or sustain life, or for any other application in which the failure of the Motorola product could create a situation where personal injury or death may occur. Should Buyer purchase or use Motorola products for any such unintended or unauthorized application, Buyer shall indemnify and hold Motorola and its officers, employees, subsidiaries, affiliates, and distributors harmless against all claims, costs, damages, and expenses, and reasonable attorney fees arising out of, directly or indirectly, any claim of personal injury or death associated with such unintended or unauthorized use, even if such claim alleges that Motorola was negligent regarding the design or manufacture of the part. M is a registered trademark of Motorola, Inc. Motorola, Inc. is an Equal Opportunity/Affirmative Action Employer. TO OBTAIN ADDITIONAL PRODUCT INFORMATION: USA/EUROPE: JAPAN: HONG KONG: MFAX: INTERNET: Motorola Literature Distribution; P.O. Box 20912; Phoenix, Arizona 85036. 1-800-441-2447 Nippon Motorola Ltd.; Tatsumi-SPD-JLDC, Toshikatsu Otsuki, 6F Seibu-Butsuryu-Center, 3-14-2 Tatsumi Koto-Ku, Tokyo 135, Japan. 03-3521-8315 Motorola Semiconductors H.K. Ltd.; 8B Tai Ping Industrial Park, 51 Ting Kok Road, Tai Po, N.T., Hong Kong. 852-26629298 RMFAX0@email.sps.mot.com - TOUCHTONE (602) 244-6609 http://www.mot.com