Peter I. Corke - Pumaservo - The Unimation Puma servo system.rar (Peter I. Corke - Pumaservo - The Unimation Puma servo system), страница 9

Thismodel is used extensively in later chapters when investigating the behaviour of visual-loop closedsystems.3.6.1 Host current control modeThe Unimate digital servo board also allows the motor current to be controlled directly by thehost. In this mode the DAC output voltage is connected directly to the current loop as shown inFigure 3.19. The position loop is inactive after a CURMODE command, and the velocity loopis disabled by deasserting /SERVO ENABLE.

The command voltage from the D/A converteris fed straight to the current loop summing junction via an analog switch enabled by the/CURRENT MODE signal. The gain of the D/A is 10=2048, and the gain of the current loopis given in Table 3.5. The DAC is updated within Tservo of the setpoint being given. This modeis useful when the host computer implements its own axis control strategy.3.6.2 LSB servo modeA curious feature of the servo system is the so-called LSB servo mode, controlled by the/LSB SERVO MODE output of the digital servo board. This feature is not used by the existingdigital servo rmware.A voltage, strongly proportional to the displacement from the current encoder edge is synthesized from the analog quadrature encoder signals by U5, U6 and U8.

This exerts a restoringforce to move the shaft back to the encoder edge, achieving levels of accuracy not possibledigitally due to quantization eects.41ed1+Encoder SetpointSumsetpoint interpolate1ZOH-KKpVDACDAC2Integralenabledabs(u)<10On-station?em-303(s+1)Switch Integral0Constant13(s+37.3)(s+373)++Sum1-10GainLead networkMuxvloopMuxVel. loop1Motorangle(rad)DeMuxDemux-KEncoderEncoderquantizationFigure 3.18: SIMULINK model POSLOOP: Unimation digital position control loop.Digital servo boardedAnalog servo boardPositionloopDACV DACLead/lag+velocityloopS2S1S2VIVIemCounterΩ̂ mdCur.loopMPowerampImRSmDigitaltachoEncoderFigure 3.19: Block diagram of Unimation servo current control mode. The switch S2 is used toroute the host setpoint directly to the current loop demand via the DAC, bypassing the velocityloop and lead compensator.3.6.3 Servo jitterIn position mode the motor shaft is generally not stationary, but vibrating at relatively highfrequency.

This is likely to be due to the servo loop interacting with non-linear friction forces onthe motor shaft, or quantization eects in the digital controller. This low level, high frequencyvibration can cause signicant contributions to end-eector sensed force and acceleration.42Appendix AThe Puma 560 manipulatorA.1 Kinematic parametersThe Puma 560 kinematics are dependent upon four non-zero length parameters and a numberof axis twist angles which are taken as either exactly 0 or exactly 90 .These lengths, which may be measured directly, are:1.

Distance between shoulder and elbow axes along the upper arm link2. Distance from the elbow axis to the center of spherical wrist joint along the lower arm3. Oset between axis of joint 4 and the elbow4. Oset between the waist axis and the joint 4 axis.The kinematic parameters for the Puma 560 are given in Table A.1. Since there are manyways of assigning coordinate frames to the manipulator, a set of kinematic parameters must betaken in the context of a particular set of axis and angle conventions; we use the conventionsof Paul and Zhang [15].

The values used are negative compared to others such as Lee [10].This is due to the denition of a right-handed conguration for the zero joint angle pose usedby Paul and Zhang. The transform T6 is considered as the position/orientation of the centerof the wrist mechanism. The distance from wrist center to the ange plate is given by Lee [10]as 56.25mm.Typically the robot pose for zero joint angles is a left-handed conguration with the upperarm horizontal along the X axis and the lower arm vertically upwards. However, Paul andZhang adopt a similar pose but in a right-handed conguration.

Thus by their convention,the upright (VAL READY) position is given by = [0 90 90 0 0 0] whilst for others it is = [0 90 90 0 0 0].i i AiDi1 90 002 0 431.8 03 -90 19.1 125.44 90 0431.85 -90 006 0 00Table A.1: Kinematic constants for Puma 560. in degrees, A and D are in mm.43Joint Armstrong [2] BreakingAway [21]G1 62.61-62.6111G2 107.36107.815G3 53.69-53.7063G4 76.0176.03636G5 71.9171.923G6 76.6376.686G451=G5G461=G6G5613=72Table A.2: Puma 560 gear ratiosA.2 Gear ratiosThe sign of the ratio indicates the direction of joint angle, , change given a positive increasein digital position loop encoder count.

In servo current control mode a positive current causesmotion in the negative encoder count direction. It seems likely that Armstrong et al. [2] havegiven only the absolute value of gear ratio. Nagy's [14] gear ratios for the Kawasaki Puma 560,agree with [21].The cross-coupling values have been determined from examination of the wrist mechanism,and correspond with numerical values given by [21]. The relationship between joint and motorangles is2j =6666666641G10000000 G10 0 G10 0 0 G10 0 0 GG0 0 0 GG23445446400001G5G56G5000001G63777777775mA.3 EncodersJoint Encoder resolution Counts/motor rev.

Counts/radl125010009965220080013727325010008548425010001210252501000114476250500y6102Table A.3: Puma 560 joint encoder resolution. yNote that joint 6 is generally run in `divide bytwo mode', so that the encoder count maintained by the digital servo board is half the numberof actual encoder counts.

This is necessary since joint 6 can rotate through a range of morethan 216 counts.44Appendix BEncoder calibrationAt reset all joint encoders are set to 0x8000, the middle of the 16 bit unsigned integer range.By convention this encoder value corresponds to the robot pose where the robot arm is vertical(VAL ready position).The incremental encoders respond only to change in position { the absolute encoder valuescorresponding to the pose of the robot at startup are not known and are determined by thecalibration procedure.Each joint also has a potentiometer sensor, which is a low resolution absolute shaft angleencoder.

The calibration procedure involves1. moving each joint to a zero index mark (one per motor revolution) using the `stop at zeroindex' control bits in the joint status word.2. reading the potentiometer sensor, 1 , and determining the approximate encoder valuefrom the known potentiometer response;e^i = ii + iwhere i and i , are experimentally determined potentiometer characteristics.3. We know that the encoder value is of the formEFIRST + n EDELTAwhere EFIRST is the encoder count at the rst zero index away from the `READY'position, EDELTA is the number of encoder counts between zero indices, and n is aninteger.

Thus given an approximate encoder count, e^, derived from the potentiometerabove, the exact count is given byEFIRST )EDELTA + EFIRSTe = nint( e^ EDELTAwhere nint(x) gives the integer nearest to x.4. Finally, the encoder registers in the digital servo boards are set to their correct value withthe SETPOS command.The coecients for each potentiometer, i and i , are determined experimentally. Theprocedure involves1. calibrating the robot as accurately as possible, generally via the engraved markings oneach joint, or using plumb bobs, levels and theodolites1To reduce the eects of quantization error, is often the sum of N repeated readings452. measuring i and ei at every zero index over the working range of the joint3. performing a regression on the i , ei pairs.The potentiometer calibration procedure need only be done once, and repeated only aftermodication or repair to any of the joint motor/potentiometer assemblies.46Appendix CCommunications protocolThe following `C' code illustrate the lowest level primitives required for communications with theUnimate servo subsystem.

The argument cmd contains the command code, and for non-vectoroperations also the joint address.In this example the interface is memory mapped, with one word for data read/write, andone word for reading/writing status lines to the AIB.#define TIMEOUT#define NLOOPS50128#define VECBIT0x80#define DATA#define COMMANDpp->status|=STATCSR0pp->status&=~STATCSR0#define OUTW(w)#define INW(w)pp->data = wpp->datastruct port {unsigned shortunsigned short};data;status;/* define status word bits */#define#define#define#defineSTATREQASTATREQBSTATCSR0STATCSR1struct port0x010x020x040x08*pp = HOSTPORTADDR;/** return the result of `cmd' applied to a single joint*/ReadSingle(cmd)intcmd;{intr;47COMMAND;OUTW(cmd);DATA;if ((cmd&7) != SELECT7)ready("rsingle2", cmd);r = INW;if ((cmd&7) != SELECT7)ready("rsingle3", cmd);return r&0xffff;}/** execute `cmd' on a single joint*/WriteSingle(cmd, data)intcmd;unsigned intdata;{COMMAND;OUTW(cmd);DATA;OUTW(data);if ((cmd&7) != SELECT7)ready("wsingle", cmd);}/** return the result of `cmd' applied to all joints*/ReadVector(cmd, vec)intcmd;unsigned int*vec;{inti, j;COMMAND;OUTW(cmd|VECBIT);DATA;for (i=0; i<NJOINTS; i++) {ready("rvector", cmd);vec[i] = INW;}}/** execute `cmd' on all joints, different data for each joint*/WriteVector(cmd, vec)intcmd;48unsigned int{int*vec;i;COMMAND;OUTW(cmd | VECBIT);DATA;for (i=0; i<NJOINTS; i++) {OUTW(cmd==STOPMDE ? 0 : vec[i]);ready("wvector", cmd);}}/** execute `cmd' on all joints, same data for each joint*/WriteMultiple(cmd, val)intcmd;unsigned intval;{inti;COMMAND;OUTW(cmd | SEQBIT);DATA;for (i=0; i<NJOINTS; i++) {OUTW(cmd==STOPMDE ? 0 : val);ready("wmultiple", cmd);}}/** return attention and joint tolerance bits*/GetTol(){COMMAND;OUTW(0);return INW;}/** wait*/staticcharint{until servos acknowledge command completionready(where, cmd)*where;cmd;inttimeout;49timeout = TIMEOUT;while ((pp->status & STATREQA) == 0 && --timeout > 0);if (timeout == 0) {printf("joint timeout from %s, cmd = %x\n", where, cmd);return;}}C.1 InitializationBefore the digital servos can be used some initialization is required by the host.1.