Peter I. Corke - Pumaservo - The Unimation Puma servo system.rar (779752), страница 4
Текст из файла (страница 4)
The code is written in a `C' like pseudo-code and covers control strategy,interrupt front end, and host command handling. The commented disassembly, in a formfor reassembly can be obtained by anonymous ftp from asgard.mlb.dmt.csiro.au in the lepub/pic/servo.s.Z.2.3.1 Position control strategyThis function, summarized in Figure 2.8, is quite straightforward. However the implementation is complex since it is mostly done with triple precision, xed point arithmetic.
The leastsignicant byte is considered as being to the right of the binary point. It performs setpointinterpolation at the 6503 tick rate, as well as implementing the position control loop. It iscalled every 924 s and updates the current setpoint e0d by an amount e . This increment iscomputed each time the host provides a new setpoint but the setpoint itself is not actually kept,see Section 2.3.3. Each cycle the current arm position is read by getenc() and the result placedin a circular buer, pos, with PBUF elements.The position control algorithm implemented is a simple digital PD law10 e0 (k) e (k) + D (e (k) e (k velgan))vDAC (k) = 2048(2.1)mmmdwhere e0d is the desired encoder value from the interpolator, and em is the current measured encoder count.
This control law has been veried by disassembly of the microprocessor's EPROMand by direct measurement of the transfer function VDAC =Em. The second term above is a18#define PBUF16pos_control(){i = (i - 1) % PBUF;pos[i] = getenc();/* put encoder value into pos buffer */if (CR & SERVO_ENABLE) {e_d += e_delta;/* update target position */if (--count == 0)e_delta = 0;/* no movement if past count */err = pos[i] - e_d;/** set TOL and INT bits*/if (abs(err - postol))TOL = 1;elseTOL = 0;if (abs(err - inttol))INT = 1;elseINT = 0;/** PD control*/u = P * err + dcoffset;u >>= outscal;j = (i + velgan) % PBUF;deriv = pos[i] - pos[j]; /* compute deriv fb */u += deriv;dacout(u);pending();/* handle pending commands */}else {}enc = pbuf[i];}new_setpoint(e_new){e_delta = (e_new - e_d) / ninter;count = ninter;}Figure 2.8: Position control algorithm.simple derivative obtained by taking the dierence between the current position in the circularbuer, and the velganth one before that.
The output is a velocity demand to the analog velocityloop which is described in more detail in Section 3.6.The position tolerance (SPOSTL command) is checked every tick and used to drive thejoint's TOL bus line, indicating to the host that the joint is close to its setpoint. Similarlythe integration band (SETINT command) is checked, and used to enable integral action on theanalog velocity loop when the joint is close to its target position.2.3.2 Interrupt front-endThe mainline code, shown in Figure 2.9, is invoked by a hardware reset from the bus.
Note thatthe axis processor is able to be reset by the host via the AIB.19main(){for (;;) {{initialize stack}{initialize memory}{read encoders}init_pos_buffer();while (control_bus & HALT_BIT == 0);}while (control_bus & HALT_BIT);}nmi(){{push registers}while (f = jtab1[--cmd])(*f)(arg[cmd]);{restore registers}/* while pending commands *//* call handler */}irq(){{push registers}if ({6532 timer int}) {{ clear interrupt request }{ reload interval counter}pos_control();}elsejoint_index();{restore registers}}Figure 2.9: Interrupt and main-line code.cmd7 /cmd3 cmd6 cmd5 cmd476543hltav1ar1210Figure 2.10: Transposed command bits as seen by the axis processor.
The reason for thistransposition is not known.2.3.3 Host command handlingAs mentioned above, when a command is written to a selected joint servo card an NMI isgenerated. NMIs are in fact controlled by external logic and a bit in the CR register, allowingthem to be masked o during certain critical code sections. The NMI handler uses the CMDbus value as an index into a dispatch table whose contents are used as the low byte of a jumpaddress 0x0e00 + table1[CMD].
The command handlers are executed 39 cycles (39 s) afterthe NMI is received. The format of the CMD bus value as it appears to the 6503 is not simple,as some of the bits have been transposed as shown in Figure 2.10. The handlers called fall intotwo categories:1. Those for DIAGREAD, READSTATUS, READPOS, READADC and NOP are handledimmediately, and the return values (if any) are written to the arm interface card. ForREADADC, a software timing loop delays the acknowledge to allow for the 100 s ADC20conversion time.2. Those for CURMODE, POSMODE, SETPOS, SETDC, STDATA, SPOSTL, CALIB,SETINT and STOPMDE are not handled immediately but are placed into a four deepcircular buer containing the command, and 16-bit parameter.
They are executed by thepending() function at the end of each control update. CURMODE and POSMODE willperform a NOP if servo mode is disabled in the joint status word. The 6503 acknowledgesthese commands within approximately 75 s.The pending() function will service all pending commands. The saved commands are againused to nd a handler address, but this time via a dierent dispatch table, and the completejump address is 0x0d00 + table2[CMD]. The commands may be grouped: SETPOS, SETDC, SPOSTL, SETINT, STDATA simply copy their parameter into thememory location associated with each variable. STOPMDE operates by setting the encoder demand ed to the current position, pos[i],eectively servoing to the current point, and disables current mode. CURMODE sign extends the data and outputs it to the DAC immediately.
This meansthat current commands can be output every clock tick, and will be synchronized to theclock tick. POSMODE computes the encoder increment needed at each clock tick:e_delta = (e_new - e_d) / ninterNote that if the host is sending many commands to a single axis the command buer mayoverow. The axis acknowledges a command in 75 s, but will execute them only every 924 s.In that time 12 commands could potentially be sent and exceed the 4 buers actually available.2.3.4 Joint status wordThe lowest two bytes of 6503 memory are known as the joint status word, and the READSTATcommand is available to return that as a word value.
The low byte is stored in location 0,the high byte in location 1. The joint status word is set using the STDATA command, onebyte at a time. The signicance of the bits are shown in Table 2.5 as per [21]. Our rmwareappears not to implement some status ags such as overheated motor, DAC saturated timeout,out of position enveloped and lost synchronization with encoder. DAC saturation refers to thecondition of maximum DAC output sustained for some period of time.2.3.5 TimingThe 6532 multifunction device contains an 8-bit interval counter that decrements every 1, 8,64 or 1024 2 edges.
The counter can generate an interrupt when it decrements to zero. Thecounter has no initial value register so it cannot reload and continue counting.In the joint servo card the 2 rate is 1 MHz (from the backplane clock signal generated in theclock/terminator card). The software sets the interval counter's divisor to the value containedin location 8, initialized to 0x6f or 111, and the count mode to decrement every 8 2 edges.This corresponds to an interval of 896 s,interval = (divisor + 1) factorfactor = 1; 8; 64 or 102421Bit0123456,78Value0x010x020x040x080x100x2090x10010,11121314150x10000x20000x40000x80000x80DescriptionRAM errorNo zero indexOut of position envelopeLost synchronization with encoderDAC saturated timeoutOverheated motorNot usedLook for zero index (set by user).
The jointresponds to position demands as normal, butstops moving at the rst zero index encountered,and the joint's attention line is asserted.Set by the joint when a zero-index has beenfound. Further move commands will be ignoredwhile this bit remains high.Not usedDivide-by-two modeNot usedEnable integration inside bandEnable servoing (joint will not move until this isset)Table 2.5: Joint status word.IRQ from6532executingpositionloopDAC update584us30us896usFigure 2.11: Timing details of axis processor rmware.Measurements with a logic analyzer indicate that the 6503 takes 30 s from IRQ beingasserted by the 6532 to the software reseting the IRQ ag. The software then reloads the6532 interval counter.
Thus the measured interval between IRQ edges of 924 s correspondsapproximately to 896+30 s. The DAC is updated some 584 s after IRQ is asserted as shownin Figure 2.11.2.3.6 Host synchronizationTwo variables in the 6503 memory control the number of servo cycles between host setpointcommands and these are shown in Table 2.6. Note that if a setpoint command occurs later22interval no.of ticks224 ms256112 ms12856 ms6428 ms3214 ms167 ms8intscl012345ninter2561286432168Table 2.6: Setpoint timing control.than expected the joint will stop by servoing to the last setpoint.
If host setpoints come moreslowly than expected the joints will stop before the next setpoint arrives, leading to very roughmotion. If the command comes early the encoder increment is adjusted at the next tick andservoing will continue, but at a dierent velocity.The number of ticks expected between host setpoint commands is kept in the variableninter.
Note that the tick period of 924 s is not an integral divisor of the host setpointupdate rates used. The encoder increment is scaled by intscl to suit the xed point arithmeticused in the control algorithm. Since the scaling is done by shifting, not division, ninter isrestricted to being a power of 2.
![](https://s.studizba.com/z.php?f=/templates/si/i/noavatar.png&w=100&h=100&t=1"){kind=avatar}
