Order this document MOTOROLA by AN1120/D | SEMICONDUCTOR a AN1120 Basic Servo Loop Motor Control Using the MC68HCO5B6 MCU By Jim Gray This application note describes a basic circuit and software implementing proportional derivative (PD) closed-loop speed control for a brush motor using four integrated circuits (ICs), two opto discretes, and less than 200 bytes of code. Feedback control systems using digital algorithms implemented on microcontroller units (MCUs) are becoming increasingly commonplace. The use of an MCU in this type of control application is justified when system flexibility is needed, such as varying drive motors or storing wear parameters in electri- cally erasable programmable read-only memory (EEPROM). Typically, the system would be modeled mathematically in the discrete time domain due to the use of sampled rather than continuous data. The linear difference equations describing the transfer function of the system are solved using z-transforms, allowing, in the case of proportiona!-integral-derivative (PID) control, the determination of constants for proper system performance and stability. However, this level of analysis is not neces- sary to illustrate how straightforward the implementation is using the MC68HCO5B6 and the MPM3004 TMOS H-bridge. The generalized flow of a PD loop is shown in Figure 1. The transfer function of Ge(s) consists of the PD control, and Gp(s) represents the power amplifier, motor, and load. Here s is a complex variable having both real and imaginary parts. The proportional term Kp can be accomplished with shifting operations, at least to the resolution of powers of 2. The derivative term, Kps, of f(t) is approximately GD) 1 a haat = pl kD -F (k-1)T] where f{kT) is the current value of the controlled parameter, and f{k 1)T is the value of the same parameter at the previous sampling time. In this example, kps is realized as the rate of change of the difference between the measured and the desired period of motor-shaft rotation. Figure 1. PD Loop Flow TMOS is a trademark of Motorola inc. (MA) MOTOROLA HE MOTOROLA INC., 1991The MC68HCO5B6 is an M68HC05 MCU Family member with two channels of programmable pulse- length modulation on-chip. When used with an H-bridge device such as the MPM3004, these chan- nels can control bidirectional currents of up to 10-A continuous (25-A peak) at 60 V (see Figure 2). Two I/O pins and both pulse-length modulation (PLM) channels are used to control the MPM3004. Proper gate drive and level conversion is provided by the MC34151 dual inverting gate drivers. Input to the control loop consists of the MLED71 infrared emitter and MRD750 photo Schmitt trigger detec- tor coupled through a slotted disc on the motor shaft. The TCAP2 pin and associated input capture registers are used to convert the optical index marks into a time measurement. Great care must be taken to ensure an adequate current source for the MPM3004 and to isolate the supply for the MC34151s. Separate circuit runs and 0.1-uF bypass capacitors on the MC34151 ICs were used in this case. The justification for adding a derivative term to a proportional controller can be easily understood by examining the reasons for the overshoot and ringing typical of an underdamped proportional-only controller. When proportional control applies additional power to correct an underspeed condition, it does so continuously until the error term is zero, resulting in a power setting that ensures an over- speed condition. The converse occurs when reducing motor speed. The rate of change of the error signal as excessive power is being applied to correct underspeed will be a relatively large negative value (the error term is being rapidly reduced). Thus, the derivative of the error term is of the correct sign to compensate the proportional gain term. One effect of this compensation is to retard the loops response time, but the proportional gain can be increased to offset this. The listing (see Figure 3) shows the assembly source code for speed measurement and the PD con- trol of PLMA, which drives the power H-bridge in one direction. The opposite direction of rotation is obtained by complementing bits 0 and 1 of port A and driving the opposite lower leg of the H-bridge with PLMB. Eight-bit arithmetic was used exclusively in this example for space and clarity. Although this approach is functional, 16-bit routines for multiply and divide, given in Reference 2, are better for finer control. Routines to set initial values, control direction of rotation, and check for motor stall are also necessary, although they are not shown in this application note. Figure 4 shows the response of the system to various changes in load. The data was captured in an emulator trace buffer (Motorola CDS8 Jewelbox) and plotted using a data base program. Beginning from a no-load condition at 4 s, loading (an uncalibrated friction brake) was ramped to cause approxi- mately a 50-percent duty cycle. Starting at 10 s, the load was then increased again until the system was at the limit of compliance i.e., at full power and still maintaining the desired speed. Next, at 14 s, approximately half the load was rapidly (0.1 s) removed. The gain of the proportional term was 2, and the derivative constant was 1. In systems where a low-pass filter would be beneficial or the steady state error is potentially large, an integral term could be added for full PID control. REFERENCES 1. Kuo, Benjamin C., Automatic Control Systems, New Jersey: Prentice-Hall, 1987. 2. M6805UM/AD2, M6805 HMOS/M146805 CMOS Family Users Manual, New Jersey: Prentice- Hall, 1983. 3. MC68HCO5B6/D, MC68HCO5B6 Data Sheet, Motorola, 1988. 4. M68HCO5AG/AD, M68HC05 Applications Guide, Motorola, 1989. MOTOROLA AN1120/D 2Figure 2. Block Diagram of Servo Loop Motor Control {01JUOD 10}OY doo] OAJAS jo WesBeIg YO0Ig *z aunbig IAdLNO G02 YOLOW 90 WANdLINOANYG | PD | eo 4 L_fa =p | | @.1NALNO | g | os 2S oS a JAG I! +H vooeWdl 001 4 V LNdLNO SAIHG LAGNI AAdNI wf N _ eo] st oot sty woh rt oD - NT ON al 2 a LSLECON + + -|] N c-) _ Alri na Op, mi +E N N uw e| >| ed 0Qd Od $dd 90d 20d edVOL bdWOL edWOl lau OaL 19S aWd VINId dda THA HYA bdVOL dd %d HO! 2080 SSA NOW 9E/PESOOHBSON 00d Od 60d 0d vd GOd 990d Lod 0ad Gd 2d ad bad Sad 98d 2@d OVd Wd ovd tWd vWd SVd 9Vd Nd ON ON 19SO Gd, LaS3Y 6b 8 dy 9b Sb bp & ov 6 8 Ze 9 Se be of > \e 4 < oe 0 6 82 Ze 9% G@ ve Ol MOTOROLA AN1120/D 3A KIKI II I HI Ie III IE I Fa I ae eat OI do oe ae de de ie MC68HCO5B6 SERVO LOOP MOTOR CONTROL EXAMPLE * This program performs a closed loop servo speed control using PLMA for * output. Speed is measured optically with a slotted disk. The optically * detected index mark,controls TCAP2 which allows calculation of the * period of revolution for the loop input. * * * * * * eI I Ie II I II IR I II IIE I IR I I I I ak ak i kk ik kK eK aOR RK OK ok aoar~arann PWN EE 9 0000 org . $0 10 cycles off 11 0000 12 9000 PADR RMB 1 13 0001 PBDR RMB 1 14 90002 PCDR RMB 1 15 0003 PDIDR RMB 1 16 0004 PADDR RMB 1 17 0005 PBDDR RMB 1 18 0006 PCDDR RMB 1 19 20 OO0A ORG SOA 21 22 000A PLMA RMB 1 23 0008 PLMB RMB 1 24 000C MISC RMB 1 25 26 0012 ORG $12 27 28 0012 TCR RMB 1 29 0013 TSR RMB 1 30 0014 CAHRi RMB 1 31 0015 CALRL RMB 1 32 0016 COHR1 RMB 1 33 0017 COLR1 RMB 1 34 0018 CNTHR RMB 1 35 0019 CNTLR RMB 1 36 OO1A ACNTHR RMB 1 37 001B ACNTLR RMB 1 38 001C CAHR2 RMB 1 39 O01D CALR2 RMB 1 40 41 0050 ORG $50 42 43 0050 BCNTH RMB 1 44 0051 BCNTL RMB 1 45 0052 ECNTH RMB 1 46 0053 ECNTL RMB 1 47 0054 PERIOD RMB 1 48 0055 PLMTMP RMB 1 MUST BE INITIALIZED WITH STARTING VALUE 49 0056 DESPRD RMB 1 MUST BE INITIALIZED WITH DESIRED PERIOD COUNT 50 0057 DELTAN RMB i 51 0058 DELTAO RMB 1 52 0059 DELTADC RMB 1 53 005A 54 OFOO ORG SFOO 55 56 OFOO A604 BEGIN LDA #S4 SELECT SLOW PLM REPETION RATE 57 OFO2 B7O0C STA MIsc SPEED 58 OFO04 B655 LDA PLMTMP LOAD PLM VALUE 59 OFO06 B70A STA PLMA 60 OFO8 B613 KEYS LDA TSR CLEAR FLAG AND ANY PENDING INT. 61 OFOA B61Cc LDA CAHR2 62 OFOC B61D LDA CALR2 63 OFOE 1E12 BSET 7,TCR SET INPUT CAPTURE INTERRUPT ENABLE 64 OF10 9A CLI CLEAR I BIT ALLOWING TIMER INTERRUPTS 65 OF11 20PFE WAIT BRA WAIT WAIT FOR OPTO INDEX TCIC INTERRUPT 66 OF13 B13 RPM LDA TSR CLR TSR BIT 4 TO ENSURE 67 OF15 Belc LDA CAHR2 SYNCHRONIZATION TO INDEX 68 OF17 B61D LDA CALR2 Figure 3. MC68HCO5B6 Servo Loop Motor Control Example en MOTOROLA AN1120/D 469 OF19 081302 TFLAG1 BRSET 4,TSR,INDEX1 TEST FLAG FOR INDEX1 70 OF1C 20FB BRA TFLAG1 71 OFLE B61C INDEX1 LDA CAHR2 STORE COUNT 72 OF20 B750 STA BCNTH 73 OF22 B61D LDA CALR2 74 OF24 B751 STA BCNTL 75 OF26 4F CLRA DELAY TO AVOID RETRIGGER ON SAME INDEX 76 OF27 4A DEC1 DECA 77 OF28 26FD BNE DEC1 78 OF2A B613 LDA TSR CLEAR FLAG AND WAIT 79 OF2C B61iC LDA CAHR2 FOR INDEX2 80 OF2E B61D LDA CALR2 81 OF30 081302 TFLAG2 BRSET 4,TSR, INDEX2 82 OF33 20FB BRA TFLAG2 83 OF35 B61C INDEX2 LDA CAHR2 STORE SECOND COUNT 84 OF37 B752 STA ECNTH 85 OF39 B6iD LDA CALR2 86 OF3B B753 STA ECNTL 87 OF3D B652 LDA ECNTH CALCULATE PERIOD 88 OF3F BO5O0 SUB BCNTH THEN 89 OF41 B754 STA PERIOD STORE. 90 OF43 B657 LDA DELTAN GET PREVIOUS ERROR AND 91 OF45 B758 STA DELTAO STORE IT. 92 OF47 B656 LDA DESPRD LOAD DESIRED PERIOD, SUBTRACT ACTUAL 93 OF49 BO54 SUB PERIOD TO FORM DELTAN. 94 OF4B 2529 BLO INCSPD GO TO INCREMEMTING PLM 95 OF4D 48 LSLA MULTIPLY ERROR BY 2. 96 OF4E B757 STA DELTAN OR FALL THRU TO DECREMENTING HERE. 97 OF50 B658 LDA DELTAO FORM RATE OF CHANGE 98 OF52 B057 SUB DELTAN OF ERROR 99 OF54 B759 STA DELTADC AND STORE. 100 OF56 B657 LDA DELTAN GET CURRENT ERROR 101 OF58 BO59 SUB DELTADC AND APPLY DE/DT CORRECTION 102 OFSA B759 STA DELTADC THEN STORE. 103 OF5C B655 LDA PLMTMP GET CURRENT PLM 104 OF5E BO57 SUB DELTAN AND APPLY CORRECTION. 105 OF60 2208 BHI ADJDN BRANCH TO DECREMENT IF RESULT POSITIVE 106 OF62 A610 PLMMIN LDA #S10 OTHERWISE IN LOW SATURATION SO 107 OF64 BJOA STA PLMA KEEP PLM AT MINIMUM. 108 OF66 B755 STA PLMTMP 109 OF68 2023 BRA DONE 110 OF6A A110 ADJDN CMP #S10 SEE IF PLM AT MINIMUM lil OF6C 2202 BHI DECSPD 112 OF6E 20F2 BRA PLMMIN 113 OF70 BJOA DECSPD STA PLMA DECREMENT PLMA 114 OF72 B755 STA PLMTMP UPDATE PLMA TEMPORARY LOCATION 115 OF74 2017 BRA DONE 116 OF76 48 INCSPD LSLA MULTIPLY ERROR BY 2 117 OF77 B757 STA DELTAN INCREMENT WITH SATURATION 118 OF79 B658 LDA DELTAO FORM RATE OF CHANGE 119 OF7B BO57 : SUB DELTAN OF ERROR. 120 OF7D BB57 ADD DELTAN NOW ADD IT TO CURRENT DELTA 121 OF7F B759 STA DELTADC TO FORM RATE OF CHANGE COMPENSATED ERROR. 122 OF81 B655 LDA PLMTMP GET CURRENT PLM 123 OF83 BO59 SUB DELTADC AND APPLY CORRECTION. 124 OF85 2502 BLO ADJUP 125 OF87 2004 BRA DONE IN SATURATION OR CORRECTION EQUALS 0 126 OF89 B7OA ADJUP STA PLMA 127 OF8B B755 STA PLMTMP 128 OF8D 80 DONE RTI RETURN TO WAIT 129 130 131 1LFFO ORG S1FFO set vectors 132 1FFO OFOO FDB BEGIN R 133 1FF2 OFOO FDB BEGIN SCI 134 1FF4 OFOO FDB BEGIN TOV 135 1FF6 OFOO FDB BEGIN TOC 136 1FF8 OF13 FDB RPM TIC 137 1FFA 0OFOO FDB BEGIN IRQ 138 1FFC OFOO FDB BEGIN SWI AN1120/D MOTOROLA139 1FFE 0F00 FDB BEGIN RES 140 2000 END Symbol Table: Symbol Name Value Def.# Line Number Cross Reference ACNTHR OO1A *00036 ACNTLR 001B *00037 ADJDN OF6A *00110 00105 ADJUP OF89 *00126 00124 BCNTH 0050 *00043 00072 00088 BCNTL 0051 *00044 00074 BEGIN OFOO *00056 00132 00133 00134 00135 00137 00138 00139 CAHRI1 0014 *00030 CAHR2 001C *00038 00061 00067 00071 00079 00083 CALRI1 0015 *00031 CALR2 001D *00039 00062 00068 00073 00080 00085 CNTHR 0018 *00034 CNTLR 0019 *00035 COHR1 0016 *00032 COLRi 0017 *00033 DECL OF27 *00076 00077 DECSPD OF7Q0 *00113 00111 DELTADC 0059 *00052 00099 00101 00102 00121 00123 DELTAN 0057 *00050 00090 00096 00098 00100 00104 00117 00119 00120 DELTAO 0058 *00051 00091 00097 00118 DESPRD 0056 *00049 00092 DONE OF8D *00128 00109 00115 00125 ECNTH 0052 *00045 00084 00087 ECNTL 0053 *00046 00086 INCSPD OF76 *00116 00094 INDEX1 OF1E *00071 00069 INDEX2 OF35 *00083 00081 KEYS OFO8 ~*00060 MISC go0c *00024 00057 PADDR 0004 *00016 PADR 0000 *00012 PBDDR 0005 *00017 PBDR 0001 *00013 PCDDR 0006 *00018 PCDR 0002 *90014 PDIDR 0003 *00015 PERIOD 0054 *00047 00089 00093 PLMA OOOA *00022 00059 00107 00113 00126 PLMB O0OB *00023 PLMMIN OF62 *00106 00112 PLMTMP 0055 *00048 00058 00103 00108 00114 00122 00127 RPM OF13 *00066 00136 TCR 0012 *00028 00063 TFLAG1 OF19 *00069 00070 TFLAG2 OF30 *00081 00082 T