Servo Control of DC Motor_Full Project

AN532Servo Control of a DC-Brush Motor Author: Tim Bucella Teknic, Inc. FIGURE 1: Digital Command A TYPICAL SERVO SYSTEM Motor Processor D/A M • Power • Amplifier • • E Φ2 Encoder INTRODUCTION The PIC17C42 microcontroller is an excellent choice for cost-effective servo control in embedded applications. Due to its Harvard architecture and RISC features, the PIC17C42 offers excellent computation speed needed for real-time closed loop servo control. This application note examines the use of the PIC17C42 as a DC brush motor servo controller. It is shown that a PID (Proportional, Integral, Differential) control calculation can be performed in less than 200 µs (@16 MHz) allowing control loop sample times in the 2 kHz range. Encoder rates up to 3 MHz are easily handled by the PIC17C42's high speed peripherals. Further, the on-chip peripherals allow an absolute minimum cost system to be constructed. Closed-loop servo motor control is usually handled by 16-bit, high-end microcontrollers and external logic. In an attempt to increase performance many applications are upgrading to DSPs (Digital Signal Processors). However, the very high performance of the PIC17C42 makes it possible to implement these servo control applications at a significant reduction in overall system cost. The servo system discussed in this application note uses a PIC17C42 microcontroller, a programmable logic device (PLD), and a single-chip H-bridge driver. Such a system might be used as a positioning controller in a printer, plotter, or scanner. The low cost of implementing a servo control system using the PIC17C42 allows this system to compete favorably with stepper motor systems by offering a number of advantages: • • • • Increased Acceleration, Velocity Improved Efficiency Reduced Audible Noise True Disturbance Rejection Encoder Counter Φ1 The PIC17C42 implements both the servo compensator algorithm and the trajectory profile (trapezoidal) generation. A trajectory generation algorithm is necessary for optimum motion and its implementation is as important as the servo compensator itself. The servo compensator can be implemented as a traditional digital filter, a fuzzy logic algorithm, or a simple PID algorithm (as implemented in this application note). The combination of servo compensator and trajectory calculations can place significant demands on the processor. The D/A conversion can be handled by a conventional DAC or by using the PIC17C42’s pulse-width modulation (PWM). In either case the output signal is fed to a power stage which translates the analog signal(s) into usable voltages and currents to drive the motor. PWM output can be a duty-cycle signal in combination with a direction signal or a single signal which carries both pieces of information. In the latter case a 50% duty cycle commands a null output, a 0% duty cycle commands maximum negative output, and 100% maximum positive output. The amplifier can be configured to supply a controlled voltage or current to the motor. Most embedded systems use voltage output because its simpler and cheaper. Sequential encoders produce quadrature pulse trains, from which position, speed, and direction of the motor rotation can be derived. The frequency is proportional to speed and each transition of F1 and F2 represents an increment of position. The phase of the signals is used to determine direction of rotation. These encoder signals are usually decoded into Count Up and Count Down pulses, using a small state machine. These pulses are then routed to an N-bit, up/down counter whose value corresponds to the position of the motor shaft. The decoder/counter may be implemented in hardware, software, or a combination of the two. SYSTEM OVERVIEW DC Servo Control Modern digital servo systems are formed as shown in Figure 1. These systems control a motor with an incremental feedback device known as a sequential encoder. They consist of an encoder counter, a processor, some form of D/A (Digital-to-Analog) converter, and a power amplifier which delivers current or voltage to the motor. © 1997 Microchip Technology Inc. DS00532C-page 1 AN532 The PIC17C42 Based Motor Control Board The PIC17C42 based servo system described here has a full RS-232 ASCII interface, on-board switching power supply, H-bridge motor drive, over-current protection, limit switch inputs and digital I/O. The entire system measures 5” x 3.5” and is shown in Figure 2. The system can be used to evaluate the PIC17C42 in servo applications. All unused PIC17C42 pins are available at an I/O connector for prototyping. FIGURE 2: THE PIC17C42 BASED SERVO CONTROL BOARD DS00532C-page 2 © 1997 Microchip Technology Inc. AN532 A PID algorithm is used as a servo compensator and position trajectories are derived from linear velocity ramp segments. This system uses 50%-null PWM as the D/A conversion technique. The power stage is a high current output switching stage which steps-up the level of the PWM signal. Encoder signal decoding is accomplished using an external PLD. The up/down counter is implemented internally in the PIC17C42 as combination of hardware and software (Figure 3 and Figure 4). FIGURE 3: Φ1 Φ2 1x mode up_count down_count 4x mode up_count down_count SEQUENTIAL ENCODER SIGNALS FIGURE 4: ENCODER INTERFACE SCHEME TMR0 up-count 4 RA1/T0CKI 16-bit counter PLD 16R8 TMR3 down-count RB5/TCLK3 16-bit counter PIC17C42 19 5 Φ2 18 Φ1 1 1x/4x select © 1997 Microchip Technology Inc. DS00532C-page 3 AN532 THE COMPENSATOR A PID routine is the most widely used algorithm for servo motor control. Although it may not be the most optimum controller for all applications, it is easy to understand and tune. The standard digital PID algorithm’s form is shown in Figure 5. U(k) is the position or velocity error and Y(k) is the output. This algorithm has been implemented using the PIC17C42’s math library. Only 800 instruction cycles are required, resulting in a 0.2 ms PID execution time at 16 MHz. Integrator windup is a condition which occurs in PID controllers when a large following error is present in the system, for instance when a large step disturbance is encountered. The integrator continually builds up during this following error condition even though the output is saturated. The integrator then “unwinds” when the servo system reaches its final destination causing excessive oscillation. The PID implementation shown in Figure 5 avoids this problem by stopping the action of the integrator during output saturation. MOTOR ACTUATION The PIC17C42 contains a high-resolution pulse width modulation (PWM) subsystem. This forms a very efficient power D/A converter when coupled to a simple switching power stage. The resolution of the PIC17C42 PWM subsystem is 62.5 ns (at 16 MHz). This translates into 10-bit resolution at a 15.6 kHz rate or 1 part in 800 (9 1/2-bit) resolution at 20 kHz. This allows effective voltage control while still maintaining the modulation frequency at or above the limit of human hearing. This is especially relevant in office automation equipment where minimizing noise is a design goal. The motor responds to a PWM output stage by time averaging the duty cycle of the output. Most motors react slowly, having an electrical time constant of 0.5 ms or more and a mechanical time constant of 20.0 ms or more. A 15 kHz PWM output is effectively equivalent to that of a linear amplifier. In the system shown in Figure 6, the H-bridge’s direction input is wired directly to the PIC17C42’s PWM output. The H-bridge is powered by a DC supply voltage, Vm. In this configuration 0 volts is presented to the motor when the PWM signal is at a 50% duty cycle, -Vm volts at 0% duty cycle and +Vm volts at 100% duty cycle. FIGURE 5: DIGITAL PID IMPLEMENTATION Optional Anti-Windup Logic + + U(k) Σ Z-1 I + + P + Z-1 + -1 Σ Y(k) Saturation To PWM Σ D Note: The Z operator indicates a one sample time delay FIGURE 6: THE PIC17C42 SERVO SYSTEM +5 Vm LMD18201 Out1 Dir +5 PIC17C42 PWM Out2 Brake GND 16R8 PLD Φ1 Φ2 E M T0CKI TCLK3 TCLK12 RX, TX = 16-bit timer input = 16-bit timer input = 8-bit timer input = serial port receive and transmit pins Serial Command RX TX PWM1 PWM2 TCLK12 Non-Servo I/O • • • T0CKI TCLK3 CLKIN CLKOUT CountUp CountDn 16 MHz Osc. DS00532C-page 4 © 1997 Microchip Technology Inc. AN532 ENCODER FEEDBACK Position feedback for the example system is derived from a quadrature encoder mounted on the motor shaft. Both incremental position and direction can be derived from this inexpensive device. The quadrature encoder signals are processed by a 16R8-type PLD device as shown in Figure 6. The PLD converts the quadrature pulses into two pulse streams: Count Up and Count Down (Figure 3). These signals are then fed to two 16-bit timers of the PIC17C42 (Timer3 and Timer0). A logic description for the PLD decoder is shown in Appendix B. The PIC17C42 keeps track of the motor shaft’s incremental position by differencing these two 16-bit timers. This operation is performed each servo sample time and the current position is calculated by adding the incremental position to the previous position. Since both timers are 16-bits, keeping track of the overflow is unnecessary, unless the encoder signals frequency is greater than 32767 times the sample frequency. For example, at a servo sample time of 1 ms, the maximum encoder rate would be 3.2767 MHz. Counter wraparound is not a concern because only the difference between the two counters is used. Two’s-complement subtraction takes care of this automatically. Position is maintained as a three-byte, 24-bit quantity in the example program shown in Appendix F. However, there is no limit to the size of the internal position register. By adding the 16-bit incremental position each sample time to an N-byte software register, an N-byte position may be maintained. TRAJECTORY GENERATION A trajectory generation algorithm is essential for optimum motion control. A linear piecewise velocity trajectory is implemented in this application. For a position move, the velocity is incremented by a constant acceleration value until a specified maximum velocity is reached. The maximum velocity is maintained for a required amount of time and then decremented by the same acceleration (deceleration) value until zero velocity is attained. The velocity trajectory is therefore trapezoidal for a long move and triangular for a short move where maximum velocity was not reached (Figure 7). The doPreMove subroutine is invoked once at the beginning of a move to calculate the trajectory limits. The doMove routine is then invoked at every sample time to calculate new “desired” velocity and position values as follows: VK = VK-1 + A (A = Acceleration) PK = PK-1 + VK-1 + A/2 For more details on trajectory generation, see Appendix E. FIGURE 7: VELOCITY RAMP SEGMENTS FOR POSITION MOVES Velocity Velocity Limit Short Move Long Move Slope = Accel. limit Time © 1997 Microchip Technology Inc. DS00532C-page 5 AN532 IMPLEMENTATION DETAILS The program structure is straightforward: An interrupt service routine (ISR) processes the servo control and trajectory generation calculations, and a foreground loop is used to implement the user interface, serial communication, and any exception processing (i.e., limit switches, watchdog timer, etc.). The ISR has a simple structure. In order to effect servo control we need to read the encoder, calculate the new trajectory point and PID values, and set the output of the PWM, all at a constant, predefined rate. The ISR is initiated by a hardware timer (Timer2) on the PIC17C42. To make sure that the servo calculation always occurs synchronously with the PWM subsystem, the PWM2 output is wired to the input pin of TMR12 (TMR1 in internally-clocked, 8-bit timer mode; TMR2 in externally-clocked, 8-bit counter mode). N is loaded into the PR2 register. The sample rate then becomes the PWM rate divided by N. In this implementation N = 16 (Figure 8). FIGURE 8: SAMPLING SCHEME TMR1x8 PWM output 15.625 kHz PWM1 PWM2 Reset TMR2x8 Servo-update interrupt 0.9765625 kHz Comparator PR2 Period = 16 PIC17C42 16 PWM cycles = 1.024 ms 16 PWM cycles 1 PWM cycles = 64 µs Servo-update interrupt DS00532C-page 6 © 1997 Microchip Technology Inc. AN532 FIGURE 9: FLOWCHART FOR FOREGROUND PROCESSING DCMOTOR.ASM IntPoll Program setup FIGURE 10: FLOWCHART FOR INTERRUPT SERVICE ROUTINE Save Registers doMposMvel • Read up_count and down_count • Measure current velocity, position IdleFunction GetChk doExtStat • Monitor external status inputs Got a char? No Move Running? Yes No New Move? No Yes doPreMove Get Command No In list? Yes Yes Trajectory in Progress? No doMove Next Command Execute Command Function doError • Computer position and velocity error No End of list? Yes Send ERROR message Send response message Servo on? Yes doServo • Compute PID • Compute PWM value • Write PWM value doCaptureRegs • For PICMASTER based debug • Output position, velocity, etc. info CAPFLAG? Yes Restore Registers RETURN © 1997 Microchip Technology Inc. DS00532C-page 7 AN532 The following events must occur in the interrupt service routine: • Read Timers (TMR0 & TMR3) • Calculate the new Reference Position using the Trajectory Generation Routine. • Calculate Error: U(k) = Reference Position - Current Position • Calculate Y(k) using PID • Set PWM output • Manage other housekeeping tasks (i.e. service serial characters) The entire ISR requires only 0.250 ms to execute with 16 MHz processor clock frequency. Read Parameter: R, (#) [00h to FFh] Returns the present value of a parameter. Shutter: C Returns the time (in sample time counts 0 to 65,53610) since the start of the present move and captures the commanded and actual values of position and velocity at the time of the command. Read commanded position: P Returns the commanded position count which was captured during the last Shutter command. Range: -8,388,60810 to 8,388,60710. Read commanded velocity: V Returns the commanded velocity multiplied by 256 which was captured during the last Shutter command. Range: -8,388,60810 to 8,388,60710. Read actual position: p Returns the actual position count which was captured during the last Shutter command. Range: -8,388,60810 to 8,388,60710. Read actual velocity: v Returns the actual velocity multiplied by 256 which was captured during the last Shutter command. Range: -8,388,60810 to 8,388,60710. External Status: Returns a two digit hex number which defines the state of the bits in the external status register. Issuing this command will clear all the bits in the external status register unless the event which set the bit is still true. The bits are defined in Table 2. COMMAND INTERFACE The following commands are implemented and recognized by the user interface in the foreground loop. Move (Value): M, [-8,388,60810 to 8,388,60710] Commands the axis to move to a new position or velocity. Position data is relative, velocity data is absolute. Position data is in encoder counts. Velocity data is given in encoder counts per sample time multiplied by 256. All moves are performed by the controller such that velocity and acceleration limits set into parameter memory will not be violated. All move commands are kept in a one deep FIFO buffer. The command in the buffer is executed as soon as the executing command is complete. If no move is currently executing the commanded move will start immediately. Mode: O, (Type), [P,V, T] An argument of “P” will cause all subsequent move commands to be incremental position moves. A “V” argument will cause all subsequent moves to be absolute velocity moves. A “T” argument sets a “Torque mode’” where all subsequent M commands directly write to the PWM. This is useful for debug purposes. Set Parameter: S, (#,Value) [00h to FFh, -8,388,60810 to 8,388,60710] Sets controller parameters to the value given. Parameters are shown in Table 1. TABLE 2: EXTERNAL STATUS REGISTER BITS bit 7 bit 6 bit 5 bit 4 bit 3-0 index marker detected +limit reached -limit reached input true N/A TABLE 1: PARAMETERS Move Status: Y Parameter Velocity Limit Acceleration Limit Kp: Proportional Gain Kd: Differential Gain Ki: Integral Gain # 00h 01h 02h 03h 04h Range 0 to 8,388,60710 * 0 to 8,388,60710 ** -3276810 to 3276710 -3276810 to 3276710 -3276810 to 3276710 Returns a two-digit hex number which defines the state of the bits in the move status register. Issuing this command will clear all the bits in the move status register unless the event which set the bit is still true. The bits are defined in Table 3. TABLE 3: MOVE STATUS REGISTER BITS bit 7 bit 6 bit 5-0 move buffer empty move complete N/A * (counts per sample time multiplied by 256) ** (counts per sample time per sample time multiplied by 256) DS00532C-page 8 © 1997 Microchip Technology Inc. AN532 Read Index position: I Returns the last index position captured in position counts. Set Position (Value): H, [-8,388,60810to8,388,60710] Sets the actual and commanded positions to the value given. Should not be sent unless the move FIFO buffer is empty. Reset: Z Performs a software reset. FIGURE 11: 2250 2200 Position 2150 2100 2050 2000 0 TYPICAL SERVO RESPONSE Position Response Desired/Actual Position Capture Servo-Response: c (#Count) The c command will set a flag inside indicating that starting with the next M (servo move) command, velocity and position information will be sent out (by invoking the doCaptureRegs procedure) during every servo-loop for #count times. At the end of the #count, the processor will halt (see doCaptureRegs procedure). This is useful for debug purposes. Kp = 2048 Kd = 20480 Ki = 1024 Actual - - Desired  50 100 Time (ms) 150 Position Error Position Error 15 10 5 0 -5 -10 -15 -20 -25 Disable Servo: s This command disables servo actuation. The servo will activate again with the execution of the next M (move) command. This is useful for debug purposes. Examples: Z OV ;Reset software (No <CR> required) ;Set velocity servo mode ;(No <CR> required) M 1000<CR> ;Set velocity to 1000 M-1000<CR> ;Set velocity to 1000 in reverse ;direction Error 20 40 60 80 100 120 140 160 Time (ms) Kp = 2048 Kd = 20480 Ki = 1024 OPTIMIZING THE SYSTEM Once the PID loop is successfully implemented, the next challenge is to tune it. This was made simple through extensive use of the PICMASTER™ In-Circuit Emulator for the PIC17C42. The PICMASTER is a highly sophisticated real-time in-circuit emulator with unlimited break-point capability, an 8K deep trace buffer and external logic probes. Its user interface software runs under Windows® 3.1 with pull-down menus and on-line help. The PICMASTER software also supports dynamic data exchange (DDE). The DDE makes it possible to send its trace buffer information to a spreadsheet, such as EXCEL®, also running under Windows. To tune the PID, first a small amount of diagnostics code is added in the servo routine (doCaptureRegs). This code simply outputs, at every sample point, the actual and desired position values, actual and desired velocity values, position error and velocity error by using a TABLWT instruction. These are captured in the trace buffer of the emulator. The 'trace' condition is set up to only trace the data cycles of the 2-cycle TABLWT instructions. Next, the trace buffer is transferred to EXCEL and the various parameters are plotted. The plots graphically show the amounts of overshoot, ripple and response time. By altering Kp, Ki and Kd, and plotting the results, the system can be fine tuned. Desired/Actual Velocity 20 16 Velocity 12 8 4 0 -4 20 40 80 100 120 140 160 Time (ms) Velocity = counts/samples 60 Velocity Response Kp = 2048 Kd = 20480 Ki = 1024 Actual - - Desired  Velocity Error Velocity Response 4 2 Velocity 0 -2 -4 -6 -8 Kp = 2048 Kd = 20480 Ki = 1024 20 40 60 80 100 120 140 160 Time (ms) Velocity = counts/samples © 1997 Microchip Technology Inc. DS00532C-page 9 AN532 Under Windows multi-tasking environment, using a PICMASTER emulator, this can be done in real time as described below. Three sessions are set up under Windows: 1. A terminal emulator session to send commands to the motor control board. The “terminal” program provided with Windows is used, although any communications software such as PROCOMM will work. Second, a PICMASTER emulation session is invoked. The actual PIC17C42 is replaced in-circuit by the emulator probe. Within the emulator, trace points are setup to capture the actual and desired position and velocity values on appropriate bus cycles. Third, a session of EXCEL is started and dynamically linked to the PICMASTER sessions such that whenever the trace buffer is full, the data is sent over to EXCEL. A few simple filtering commands in EXCEL are used to separate the various data types, i.e. actual position data from desired position from actual velocity etc. Next, various plot windows are set up within EXCEL to plot these information. CONCLUSIONS Using a high-performance 8-bit microcontroller as the heart of a servo control system is a cost-effective solution which requires very few external components. A comparison with a popular dedicated servo-control chip, is presented in Table 4. TABLE 4: SERVO CONTROL CHIP COMPARISON LM629 @8 MHz Max Encoder Rate Servo Update Time 1 MHz PIC17C42 PIC17C42 @16 MHz @25 MHz 3.3 MHz 0.25 ms 2-3 kHz 4.5 MHz 0.16 ms 4-5 kHz 2. 3. Max Sampling 4 kHz Frequency Once these setups have been done, for every servo move, the responses are automatically plotted. It is then a simple matter of varying the PID coefficients and observing the responses to achieve the desired system response. At any point, the responses can be stored in files and/or printed out. Except for very long “move” commands, most position and velocity commands are executed (i.e. system settled) in less than 500 samples, making it possible to capture all variables (actual and desired position and velocity, and position errors and servo output) in PICMASTER’s 8K trace buffer. Also apparent in the comparison table is the additional processing power available when using the microcontroller. This processing can be used to provide a user interface, handle other I/O, etc. Alternatively, the additional processing time might be used to improve the performance of compensator and trajectory generation algorithms. A further advantage is that for many embedded applications using motor control the microcontroller proves to be a complete, minimum cost solution. Credit This application note and a working demo board has been developed by Teknic Inc. Teknic (Rochester, N.Y.) specializes in Motor Control Systems. References 1.Thomas Bucella, “Comparing DSPs to Microprocessors in Motion Control Systems-Some Real World Data”, PCIM conference proceedings © 1990 Intertec Communications, Inc. 2.David M. Auslander, Cheng H. Tham, “Real-Time Software for Control” © 1990 Prentice-Hall, Inc., Englewood Cliffs, NJ 3.“DC Motors, Speed Controls, Servo Systems” Fifth Edition © 1980 Electro-Craft Corporation, Hopkins, MN DS00532C-page 10 © 1997 Microchip Technology Inc. AN532 APPENDIX A: SCHEMATIC DIAGRAM © 1997 Microchip Technology Inc. T0CKI DS00532C-page 11 AN532 APPENDIX A (CONT.): SCHEMATIC DIAGRAM DS00532C-page 12 © 1997 Microchip Technology Inc. AN532 APPENDIX A (CONT.): SCHEMATIC DIAGRAM © 1997 Microchip Technology Inc. DS00532C-page 13 AN532 APPENDIX B: Combination quadrature decoder and input synchronizer. allows 1x decoding or 4x decoding based on the X4 pin. * Ver 1.0 - November 8, 1991 } MODULE QuadDivider; TITLE QuadDivider V1.0; COMMENT Device: 16R8; TYPE MMI 16R8; INPUTS; RESET NODE[PIN2] INVERTED; X4 NODE[PIN3]; P0 NODE[PIN4]; P90 NODE[PIN5]; INDX NODE[PIN6]; { Feedback pins } S2 NODE[PIN12]; S4 NODE[PIN13]; P0D NODE[PIN14]; P90D NODE[PIN15]; CntUp NODE[PIN18]; CntDn NODE[PIN19]; UP NODE[PIN16]; COUNT NODE[PIN17] INVERTED; OUTPUTS; S2 NODE[PIN12]; S4 NODE[PIN13]; P0D NODE[PIN14]; P90D NODE[PIN15]; CntUp NODE[PIN18]; CntDn NODE[PIN19]; UP NODE[PIN16]; COUNT NODE[PIN17] INVERTED; TABLE; S2 := P0D & !RESET; S4 := P90D & !RESET; P0D := P0 & !RESET; P90D := P90 & !RESET; CntUp := COUNT & UP; CntDn := COUNT & !UP; COUNT := ( P0D & S2 & !P90D & S4 & X4{ C1 } +!P0D & !S2 & P90D & !S4 { C2 } +!P0D & S2 & !P90D & !S4 & X4{ C3 } + P0D & !S2 & P90D & S4 & X4{ C4 } + P0D & S2 & P90D & !S4 & X4{ C5 } + P0D & S2 & P90D & S4 { C6 } +!P0D & S2 & P90D & S4 & X4{ C7 } + P0D & !S2 & !P90D & !S4 & X4{ C8 } ) & !RESET; UP := ( !P0D & S2 & !P90D & S4 +!P0D & S2 & P90D & S4 +!P0D & S2 & P90D & !S4 + P0D & S2 & P90D & !S4 + P0D & !S2 & P90D & !S4 + P0D & !S2 & !P90D & !S4 + P0D & !S2 & !P90D & S4 +!P0D & !S2 & !P90D & S4 ) & !RESET; END; END QuadDivider; This design { Phi0 } { Phi90 } DS00532C-page 14 © 1997 Microchip Technology Inc. AN532 APPENDIX C: C.1 PID ALGORITHM FLOWCHART Enter doServo Compute Error Value → U0 Compute proportional Y = Kp.U0 Is Y saturated? Set YPWM = 0 Is Y saturated? No Yes Bypass integral “Anti-wind-up” Convert YPWM from unipolar to bipolar Compute integral of Errror ΣU=ΣU + U0 compute integral portion Y=Y + Ki.ΣU Limit 18200 requires that PWM duty cycle is not 0% or 100% Yes YPWM = 0 No Scale Y Y = Y.2SHIFTNUM YPWM = 100 No Positive overflow in YY No Negative overflow in YY No Extract YPWM from YK YPZ=Y <23::8> Save current error value as previous error U1 = U0 Yes Set Y to max neg value = FF800000h Yes Set Y to max pos value = 007FFFFFh Yes Set YPWM = 99% Set YPWM = 1% Zero lowest 6 bits of YPWM Write YPWM high byte → PW1DCH Write YPWM high byte → PW1DCL Check external position min./max. limit inputs Return © 1997 Microchip Technology Inc. DS00532C-page 15 AN532 C.2 PID ALGORITHM CODE LISTING ;***************************************************************************** ; NAME: doServo ; ; DESCRIPTION: Performs the servo loop calculations. ; doServo MOV16 LOADAB CALL MVPF32 CLRF CPFSGT CALL LOADAB CALL ADD32 MVFP16 SUB16 MVFP16 CALL ADD32 CLRF CPFSGT GOTO MOVFP grabloop RLC32 DECFSZ GOTO grabok CLRF BTFSC GOTO poss MOVFP ANDLW IORWF CLRF CPFSGT GOTO INCF CLRF MOVLW MOVPF SETF SETF GOTO negs MOVFP IORLW ANDWF SETF CPFSLT GOTO SETF Y+B2,WREG 0x7F Y+B3 WREG Y+B3 zero6bits SATFLAG ; check if Y <= -2**23 If negative overflow, saturate y to maximum negative number Y+B2,WREG 0x80 Y+B3 WREG Y+B3 zero6bits SATFLAG Y+B3 0x7F WREG,Y+B2 Y+B1 Y+B0 zero6bits SATFLAG Y+B3,MSB negs ; ; ; ; saturate to middle 16 bits, keeping top 10 bits for PW1DCH and PW1DCL check if Y >= 2**23 If positive overflow, saturate y to maximum positive number ; if not, zero 6 bits ; if so, set Y=0x007FFFFF ; clear for debug purposes POSERROR,U0 U0,KP Dmult DPX,Y WREG SATFLAG doIntegral INTEGRAL,KI Dmult DPX,Y U0,AARG U1,AARG KV,BARG Dmult DPX,Y WREG SHIFTNUM grabok SHIFTNUM,TMP Y TMP grabloop ; save new position error in U0 ; compute KP*U0 ; Y=KP*U0 ; if previous output saturated, do ; not accumulate integrator antiwindup Basic PID calculation ; compute KI*INTEGRAL ; Y=KP*U0+KI*INTEGRAL ; compute KV*(U0-U1) ; Y=KP*U0+KI*INTEGRAL+KV*(U0-U1) ; scale Y by SHIFTNUM ; Y = Y * (2**SHIFTNUM) Scale Y ; if not, zero 6 bits ; if so, set Y = 0xFF800000 DS00532C-page 16 © 1997 Microchip Technology Inc. AN532 SETF CLRF BSF CLRF CLRF zero6bits MOV24 doTorque MOVLW ANDWF BTFSC GOTO tplimit BTFSS GOTO CLR32 GOTO tmlimit BTFSS GOTO CLR32 mplimitok MOVLW MOVPF MOVLW MOVPF ADD16 CLRF MOVLW MOVPF ADD16 testmax CLRF CLRF CLRF MVFP16 SUB24 BTFSS GOTO MOV16 GOTO testmin CLRF CLRF CLRF MVFP16 SUB24 BTFSC GOTO MOV16 limitok MOVLB MOVFP MOVFP MOV16 RETURN ;***************************************************************************** BANK3 YPWM+B0,PW1DCL YPWM+B1,PW1DCH U0,U1 ; set new duty cycle Write PWM values to PWM registers TMP+B2 YPWM+B2 YPWM+B3 YPWMIN,TMP YPWM,TMP TMP+B2,MSB limitok YPWMIN,YPWM ; check pwm minimum limit TMP+B2 YPWM+B2 YPWM+B3 YPWMAX,TMP YPWM,TMP TMP+B2,MSB testmin YPWMAX,YPWM limitok ; check pwm maximum limit ; LMD18200 must have a minimum pulse ; so duty cycle must not be 0 or 100% PWM cycle must not be 0% of 100% EXTSTAT,BIT5 mplimitok YPWM PW1DCH_INIT WREG,TMP+B1 PW1DCL_INIT WREG,TMP+B0 TMP,YPWM TMP+B1 0x40 WREG,TMP+B0 TMP,YPWM ; adjustment from bipolar to unipolar ; for 50% duty cycle Convert PWM from unnipolar to bipolar EXTSTAT,BIT6 mplimitok YPWM mplimitok Y+B3 Y+B2 Y+B2,MSB Y+B1 Y+B0 Y+B1,YPWM+B0 0xC0 YPWM+B0 YPWM+B1,MSB tmlimit ; move Y to YPWM and zero 6 bits ; entry point for torque mode If external position limits have been reached then zero PWM output ; correct by 1 LSB ; add one to bit5 of PW1DCL ; saturate to max ; saturate to min ; push errors into U(k-1) © 1997 Microchip Technology Inc. DS00532C-page 17 AN532 APPENDIX D: ENCODER INTERFACE ROUTINE ;***************************************************************************** ; NAME: doMPosMVel ; ; DESCRIPTION: Calculates current position from UpCount and DownCount ; doMPosMVel ; Do UpCounter first MVFP16 readUp MOVPF MOVPF CPFSEQ GOTO MOVPF CLRFM MOV16 SUB16 TMR0H,WREG TMR0L,UPCOUNT+B0 TMR0H readUp WREG,UPCOUNT+B1 VELOCITY+B0 UPCOUNT,MVELOCITY+B1 TMP+B0,MVELOCITY+B1 UPCOUNT,TMP+B0 ; save old upcount ; Skip next if HI hasn’t changed ; HI changed, re-read LO ; OK to store HI now ; clear bits below binary point ; compute upcount increment ; Now do DownCounter MVFP16 readDown MOVLB MOVPF MOVPF CPFSEQ GOTO MOVPF DOWNCOUNT,TMP+B0 BANK2 TMR3H,WREG TMR3L,DOWNCOUNT+B0 TMR3H readDown WREG,DOWNCOUNT+B1 ; save old downcount ;timers in Bank 2 ; Skip next if HI hasn’t changed ; HI changed, re-read LO ; OK to store HI now ; compute downcount increment MVFP16 DOWNCOUNT+B0,TMP+B2 SUB16 TMP+B0,TMP+B2 SUB16 CLRF BTFSC SETF TMP+B2,MVELOCITY+B1 MVELOCITY+B3 MVELOCITY+B2,MSB MVELOCITY+B3 ; compute new measured velocity ; sign extend measured velocity for ; 24 bit addition to measured position ADD24 MVELOCITY+B1,MPOSITION ; compute new measured position ; delta position = measured velocity RETURN ;***************************************************************************** DS00532C-page 18 © 1997 Microchip Technology Inc. AN532 APPENDIX E: doPreMove This routine is executed only once at the beginning of each move. First, various buffers and flags are initialized and a test for modetype is performed. In position mode, the minimum move is triangular and consists of two steps. Therefore, if abs (MOVVAL) > 2, an immediate move is performed. Otherwise, normal move generation is possible with the sign of the move in MOVSIGN and the appropriate signed velocity and acceleration limits in V and A, and MOVVAL/2 in HMOVVAL. In velocity mode, the sign of the move is calculated in MOVSIGN and the appropriate signed velocity and acceleration limits are placed in V and A. Finally, at modeready, MOVVAL is sign extended for higher precision arithmetic and the servo is enabled. In torque mode, MOVVAL is output directly to the PWM and the servo is disabled, and doMove is not executed. IMPLEMENTATION DETAILS OF TRAJECTORY GENERATION With the units for t in sample times, the time increment between subsequent sample times is 1, yielding the iterative equations for updating position and velocity implemented in doPosVel and given by: P ( k ) = P ( k – 1 ) + V ( k – 1 ) + A ⁄ 2, V ( k ) = V ( k – 1 ) + A where A is the signed acceleration limit calculated in doPreMove. The inverse equations of this iteration, necessary for undoing an unwanted step, are contained in undoPosVel and given by: P ( k – 1 ) = P ( k ) – V ( K – 1 ) – A ⁄ 2, V ( K – 1 ) = V ( k ) – A In position mode, the actual shape of the velocity profile depends on the values of V, A, and the size of the move. Either the velocity limit is reached before half the move is completed, resulting in a trapezoidal velocity profile, or half the move is completed before the velocity limit is realized, resulting in a triangular velocity profile. In the algorithm employed here, the velocity limit is treated as a bound on the actual velocity limit, thereby permitting exactly the same number of steps during the speedup and speed down sections of the move. Phase 1 is defined as the section of the move where the commanded position is less than half the move, and phase 2 is the remaining portion of the move. T1 is time when the actual velocity limit is reached and T2 is the time at the end of phase 1. doMove Move generation is based on a piecewise constant acceleration model. During constant acceleration, this results in the standard equations for position and velocity given by: x ( t ) = x0 + v0 × t + a × = ( t × 2 ) ⁄ 2, v ( t ) = v0 + a × t FIGURE 12: T2 x y T2+1 FIGURE 13: T2 = T1 FIGURE 14: final velocity initial velocity © 1997 Microchip Technology Inc. DS00532C-page 19 AN532 Furthermore, let x be the amount of undershoot and y the amount of overshoot of half the move at T2. Discretization error is minimized by using the values of x and y whether one more step will reduce the size of the final immediate move during the last step of the move. For a triangular move, the discretization error is given by min. (2x, 2y), resulting in the condition that if 2x > 2y, then take one more speedup step. In the case of a trapezoidal move, the discretization error is given by min. (2x, y - x), yielding the condition that if 3x > y, take one more step during the flat section of phase2. At the beginning of doMove, MOVTIME is incremented and doPosVel is called to evaluate the next proposed values of commanded position and velocity under the current value of A. In position mode, phase1, the original position plus half the move minus the new proposed commanded position is calculated and placed in MOVDEL, with the previous MOVDEL saved in MOVTMP. As half the move would be passed, MOVTMP = -x and MOVDEL = y, with y > 0 for the first time indicating that phase1 is about to be completed. Therefore, if y < 0, we continue in phase1, where if maximum velocity has not been reached, the new proposed commanded position is executed. On the other hand, if the proposed move would exceed the maximum velocity, we undo the proposed move, set the current acceleration to zero, reevaluate the iterative equations with the new acceleration, set T1 = MOVTIME - 1, and execute the move. Since T1 is cleared in doPreMove, it is used as a flag to indicate if this corner in the velocity profile has been reached. Once we find that y > 0, we drop into code that is executed only one time, with phase2 beginning on the next step. If T1 = 0, maximum velocity has not yet been reached, so T1 = T2 and the velocity profile is triangular. In this case, A is negated for speed down, and if x>y, one more step is needed to minimize the discretization error. So A is negated, the proposed step undone, A is again negated for speed down and the step recalculated and executed, with T2 = T1=MOVTIME - 1. If T1 is not zero, indicating that we are in the flat section of phase1, then go to t2net1, where T2 = MOVTIME - 1, and if 3x > y, then one more phase2 flat step is necessary to minimize the discretization error. PH2FLAT is defined as the number of steps in the flat section of phase2, and is used as a counter during its completion. If 3x > y, then PH2FLAT = T2-T1, otherwise PH2FLAT = T2-T1-1 and phase1 is finally complete. All subsequent steps will proceed through phase2, first deciding if the flat section is finished by checking if PH2FLAT has reached zero. If not, go to flat where PH2FLAT is decremented, and tested if zero. If so, the speed down section is begun by calculating the appropriate signed acceleration limit A, and executing the last of the flat section moves. For all following steps, PH2FLAT = 0, leaving only the final test for zero commanded velocity to indicate the end of the move. This will always occur since the actual maximum velocity, bounded above by the user supplied limit, is always an integer multiple of the user supplied acceleration limit, with exactly the same number of steps taken during speedup and speed down. The velocity mode is much more straightforward, with the velocity profile in the form of a ramp. If the final velocity has not been reached, the move continues at maximum acceleration. If the final velocity has been reached, the acceleration is set to zero and the move generation of commanded position and velocity continued unless the final velocity is zero. DS00532C-page 20 © 1997 Microchip Technology Inc. © 1997 Microchip Technology Inc. DS00532C-page 21 Please check the Microchip BBS for the latest version of the source code. Microchip’s Worldwide Web Address: www.microchip.com; Bulletin Board Support: MCHIPBBS using CompuServe® (CompuServe membership not required). APPENDIX F: COMPLETE CODE LISTING (DCMOTOR.LST) DCMOTOR.ASM 1-16-1997 13:20:16 PAGE 1 MPASM 01.40 Released LOC OBJECT CODE VALUE LINE SOURCE TEXT 00F42400 003D0900 000003E8 0000000C 00000019 00000067 000000CF 000000FF 00000019 00001 00002 00003 00004 00005 00006 00007 00008 00009 00010 00011 00012 00013 00014 00015 00001 00002 00003 00004 00005 00006 00007 00008 00009 00010 00011 00012 00013 00014 00015 00016 00017 00018 00019 00020 TITLE “DCMOTOR SERVO CONTROL: Revision: 1.9 ; Revised: 8/5/92 ; ; Program: DCMOTOR.ASM ; Revision Date: ; 1-13-97 Compatibility with MPASMWIN 1.40 ; ; CREDIT: Developed by Teknic Inc. 1992 ; ;***************************************************************************** ; PROCESSOR LIST PIC17C42 P = 17C42, COLUMNS=120, XREF=YES, NOWRAP, LINES=255, R=DEC #include “dcmotor.h17” ;***************************************************************************** ; ; Header file for dcmotor.asm: ; Revised: 8/5/92 ;***************************************************************************** ; ; hardware constants ; ; MASTER_CLOCK set 16000000 ; 16 MHz: change for diff clock speed CLKOUT set MASTER_CLOCK/4 SAMPLE_RATE set 1000 BAUD19200 BAUD9600 BAUD2400 BAUD1200 BAUD_MIN BAUD_DEFAULT set set set set set set (MASTER_CLOCK/((32*19200)-1)/2-1) (MASTER_CLOCK/((32*9600)-1)/2-1) (MASTER_CLOCK/((32*2400)-1)/2-1) (MASTER_CLOCK/((32*1200)-1)/2-1) 0xFF BAUD9600 AN532 DS00532C-page 22 © 1997 Microchip Technology Inc. AN532 00000006 0000003F 000000FF 0000000F 0000007F 000000C0 00000080 00000090 00000020 00000019 000000F3 00000000 00000040 00000001 00000080 000000FE 00000000 00000001 00000000 00000001 00000002 00000003 00000007 00000000 00000000 00021 00022 00023 00024 00025 00026 00027 00028 00029 00030 00031 00032 00033 00034 00035 00036 00037 00038 00039 00040 00041 00042 00043 00044 00045 00046 00047 00048 00049 00050 00051 00052 00053 00054 00055 00056 00057 00058 00059 00060 00061 00062 00063 00064 00065 00066 00067 TCON1_INIT TCON2_INIT PR1_INIT PR2_INIT PW1DCH_INIT PW1DCL_INIT RTCSTA_INIT RCSTA_INIT TXSTA_INIT SPBRG_INIT set set set set set set set set set set 0x06 0x3F 0xFF ; set pwm frequency to CLKOUT/256 khz (CLKOUT/(PR1_INIT+1)+SAMPLE_RATE/2)/SAMPLE_RATE-1 (PR1_INIT/2) ; set duty cycle to 50%, PW1DCH = PR1_INIT/2 0xC0 ; and PW1DCL = 0xC0 0x80 0x90 0x20 BAUD_DEFAULT DDRB_INIT set 0xF3 DDRD_INIT set 0x00 ; ; ; max and min pwm values ; PWMINL set 0x40 PWMINH set 0x01 PWMAXL set 0x80 PWMAXH set 0xFE ; ; ; ; 17c42 constants ; ; ; LO EQU 0 HI EQU 1 B0 EQU 0 B1 EQU 1 B2 EQU 2 B3 EQU 3 MSB EQU 7 LSB EQU 0 ; ; define special function registers: #define #define #define #define #define W 0 true false TRUE FALSE ; 0x0000 + 0x0140 (min 10 bit pwm +5) ; 0xFFC0 - 0x0140 ( max 10 bit pwm -5) 1 0 1 0 cblock 0x00 BIT0,BIT1,BIT2,BIT3,BIT4,BIT5,BIT6,BIT7 © 1997 Microchip Technology Inc. DS00532C-page 23 00000000 00000000 00000008 00000010 00000010 00000010 00000016 00000017 00000010 00068 00069 00070 00071 00072 00073 00074 00075 00076 00077 00078 00079 00080 00081 00082 00083 00084 00085 00086 00087 00088 00089 00090 00091 00092 00093 00094 00095 00096 00097 00098 00099 00100 00101 00102 00103 00104 00105 00106 00107 00108 00109 00110 00111 00112 00113 00114 endc cblock 0x00 ; define banks BANK0,BANK1,BANK2,BANK3 endc cblock 0x00 ; unbanked registers INDF0,FSR0,PCL,PCLATH,ALUSTA,RTCSTA,CPUSTA,INTSTA INDF1,FSR1,WREG,TMR0L,TMR0H,TBLPTRL,TBLPTRH,BSR endc cblock 0x10 ; bank0 registers PORTA,DDRB,PORTB,RCSTA,RCREG,TXSTA,TXREG,SPBRG endc cblock 0x10 endc cblock 0x10 endc CA1L CA1H equ equ cblock endc ;*************************************************************** ; define commonly used bits: ; ALUSTA bit definitions #define #define #define #define #define #define #define #define #define #define _carry _c _cy _dc _z _ov _fs0 _fs1 _fs2 _fs3 ALUSTA,0 ALUSTA,0 ALUSTA,0 ALUSTA,1 ALUSTA,2 ALUSTA,3 ALUSTA,4 ALUSTA,5 ALUSTA,6 ALUSTA,7 0x16 0x17 0x10 ; alternate function def ; bank2 registers TMR1,TMR2,TMR3L,TMR3H,PR1,PR2,PR3L,PR3H ; bank1 registers DDRC,PORTC,DDRD,PORTD,DDRE,PORTE,PIR,PIE ; define bank3 variables PW1DCL,PW2DCL,PW1DCH,PW2DCH,CA2L,CA2H,TCON1,TCON2 AN532 ; T0STA bit definitions DS00532C-page 24 © 1997 Microchip Technology Inc. AN532 00115 00116 00117 00118 00119 00120 00121 00122 00123 00124 00125 00126 00127 00128 00129 00130 00131 00132 00133 00134 00135 00136 00137 00138 00139 00140 00141 00142 00143 00144 00145 00146 00147 00148 00149 00150 00151 00152 00153 00154 00155 00156 00157 00158 00159 00160 00161 #define #define #define #define #define #define #define _ps0 _ps1 _ps2 _ps3 _tosc _tose _intedg T0STA,1 T0STA,2 T0STA,3 T0STA,4 T0STA,5 T0STA,6 T0STA,7 ; CPUSTA bit definitions #define #define #define #define #define _npd _nto _gint _glintd _stkav CPUSTA,2 CPUSTA,3 CPUSTA,4 CPUSTA,4 CPUSTA,5 ; INTSTA bit definitions #define #define #define #define #define #define #define #define _inte _toie _t0ckie _peie _intf _t0if _t0ckif _peif INTSTA,0 INTSTA,1 INTSTA,2 INTSTA,3 INTSTA,4 INTSTA,5 INTSTA,6 INTSTA,7 ; PIR Bit definitions #define #define #define #define #define #define #define #define _rcif _txif _ca1if _ca2if _tmr1if _tmr2if _tmr3if _rbif PIR,0 PIR,1 PIR,2 PIR,3 PIR,4 PIR,5 PIR,6 PIR,7 ; PIE Bit definitions #define #define #define #define #define _rcie _txie _ca1ie _ca2ie _tmr1ie PIE,0 PIE,1 PIE,2 PIE,3 PIE,4 © 1997 Microchip Technology Inc. DS00532C-page 25 00162 00163 00164 00165 00166 00167 00168 00169 00170 00171 00172 00173 00174 00175 00176 00177 00178 00179 00180 00181 00182 00183 00184 00185 00186 00187 00188 00189 00190 00191 00192 00193 00194 00195 00196 00197 00198 00199 00200 00201 00202 00203 00204 00205 00206 00207 00208 #define _tmr2ie PIE,5 #define _tmr3ie PIE,6 #define _rbie PIE,7 ; RCSTA bit definitions #define #define #define #define #define #define #define _rx9d _oerr _ferr _cren _cren _rx9 _spen RCVSTA,0 RCVSTA,1 RCVSTA,2 RCVSTA,4 RCVSTA,5 RCVSTA,6 RCVSTA,7 ; TXSTA bit definitions #define #define #define #define #define #define _tx9d _trmt _sync _txen _tx9 _csrc TXSTA,0 TXSTA,1 TXSTA,4 TXSTA,5 TXSTA,6 TXSTA,7 ; TCON1 bit definitions #define #define #define #define #define #define #define #define _tmr1cs _tmr2cs _tmr3cs _t16 _ca1ed0 _ca1ed1 _ca2ed0 _ca2ed1 TCON1,0 TCON1,1 TCON1,2 TCON1,3 TCON1,4 TCON1,5 TCON1,6 TCON1,7 ; TCON2 bit definitions #define #define #define #define #define _tmr1on _tmr2on _tmr3on _ca1pr3 _pwm1on TCON2,0 TCON2,1 TCON2,2 TCON2,3 TCON2,4 TCON2,5 TCON2,6 TCON2,7 AN532 #define _pwm2on #define _ca1ovf #define _ca2ovf ; ; DS00532C-page 26 © 1997 Microchip Technology Inc. AN532 0000000D 00000018 00000008 00000020 0000000A 0000002D 00000001 00000001 00000008 00000021 0000003F 00000000 00000001 00000002 00000003 00000000 00000001 00000002 00000003 0000000D 0000004D 00209 00210 00211 00212 00213 00214 00215 00216 00217 00218 00219 00220 00221 00222 00223 00224 00225 00226 00227 00228 00229 00230 00231 00232 00233 00234 00235 00236 00237 00238 00239 00240 00241 00242 00243 00244 00245 00246 00247 00248 00249 00250 00251 00252 00253 00254 00255 ; ; ascii constants ; ; CR set 0x0D CAN set 0x18 BS set 0x08 SP set 0x20 LF set 0x0A MN set ‘-’ ; ; ;***************************************************************************** ; DECIO EQU TRUE ; true for decimal, false for hex ; ; cmds constants and macros ; ; CHARREADY set 0x01 ; ; NUMPAR set 0x08 ; ; Response characters ; CMD_OK set ‘!’ CMD_BAD set ‘?’ ; ; Exit values ; ; HEX_SP set 0x00 HEX_MN set 0x01 HEX_CR set 0x02 HEX_CAN set 0x03 ; DEC_SP set 0x00 DEC_MN set 0x01 DEC_CR set 0x02 DEC_CAN set 0x03 ; ; ; Command characters ; DO_NULL set CR DO_MOVE set ‘M’ ; M © 1997 Microchip Technology Inc. DS00532C-page 27 0000004F 00000053 00000052 00000043 00000050 00000056 00000070 00000076 00000058 00000059 00000049 00000048 0000005A 00000073 00000063 00000002 00256 00257 00258 00259 00260 00261 00262 00263 00264 00265 00266 00267 00268 00269 00270 00271 00272 00273 00274 00275 00276 00277 00278 00279 00280 00281 00282 00283 00284 00285 00286 00287 00288 00289 00290 00291 00292 00293 00294 00295 00296 00297 00298 00299 00300 00301 00302 DO_MODE DO_SETPARAMETER DO_READPARAMETER DO_SHUTTER DO_READCOMPOSITION DO_READCOMVELOCITY DO_READACTPOSITION DO_READACTVELOCITY DO_EXTERNALSTATUS DO_MOVESTATUS DO_READINDPOSITION DO_SETPOSITION DO_RESET DO_STOP DO_CAPTURE set set set set set set set set set set set set set set set ‘O’ ‘S’ ‘R’ ‘C’ ‘P’ ‘V’ ‘p’ ‘v’ ‘X’ ‘Y’ ‘I’ ‘H’ ‘Z’ ‘s’ ‘c’ ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; O S R C P V p v X Y I H Z s c ;***************************************************************************** ; NAME: CMD_DEF ; ; DESCRIPTION: Creates all the definitions for a command table data struc; ture. The first word is at the command character used, and ; the second word is a pointer to the function that handles ; this command function. ; ; ENTRY CONDITIONS: Must be contiguous with the other entries for the ; function to work. ; ; ARGUMENTS: FUNC command execution function ; ROOT NAME ROOT ; CMD_DEF MACRO DATA DATA ENDM FUNC,ROOT ROOT FUNC CMD_ENTRY_LENGTH set 2 ;***************************************************************************** AN532 ;***************************************************************************** ; NAME: CMD_START ; ; DESCRIPTION: Labels the start of the command table. ; DS00532C-page 28 © 1997 Microchip Technology Inc. AN532 00303 00304 00305 00306 00307 00308 00309 00310 00311 00312 00313 00314 00315 00316 00317 00318 00319 00320 00321 00322 00323 00324 00325 00326 00327 00328 00329 00330 00331 00332 00333 00334 00335 00336 00337 00338 00339 00340 00341 00342 00343 00344 00345 00346 00347 00348 00349 CMD_START MACRO LABEL LABEL ENDM ; ;***************************************************************************** ;***************************************************************************** ; NAME: CMD_END ; ; DESCRIPTION: Marks the end of the command table with an entry of 0x00 ; CMD_END MACRO ; DATA ENDM ; 0x00 ; ;***************************************************************************** ;***************************************************************************** ; NAME: CLR32 ; ; DESCRIPTION: Clear 4 consecutive bytes of data memory ; ; ARGUMENTS: 0 => a ; ; TIMING (cycles): 4 ; CLR32 MACRO CLRF CLRF CLRF CLRF ENDM a a+B0, a+B1, a+B2, a+B3, F F F F ; ;***************************************************************************** ;***************************************************************************** ; NAME: CLR24 ; ; DESCRIPTION: Clear 3 consecutive bytes of data memory ; ; ARGUMENTS: 0 => a ; © 1997 Microchip Technology Inc. DS00532C-page 29 00350 00351 00352 00353 00354 00355 00356 00357 00358 00359 00360 00361 00362 00363 00364 00365 00366 00367 00368 00369 00370 00371 00372 00373 00374 00375 00376 00377 00378 00379 00380 00381 00382 00383 00384 00385 00386 00387 00388 00389 00390 00391 00392 00393 00394 00395 00396 ; TIMING (cycles): 3 ; CLR24 MACRO CLRF CLRF CLRF ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: CLR16 ; ; DESCRIPTION: Clear 2 consecutive bytes of data memory ; ; ARGUMENTS: 0 => a ; ; TIMING(cycles): 2 ; CLR16 MACRO CLRF CLRF ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: MOV32 ; ; DESCRIPTION: 32 bit move ; ; ARGUMENTS: a => b ; ; TIMING (cycles):8 ; a a+B0, F a+B1, F a a+B0, F a+B1, F a+B2, F AN532 MOV32 MACRO MOVFP MOVPF MOVFP MOVPF MOVFP a,b a+B0,WREG WREG,b+B0 a+B1,WREG WREG,b+B1 a+B2,WREG ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w DS00532C-page 30 © 1997 Microchip Technology Inc. AN532 00397 00398 00399 00400 00401 00402 00403 00404 00405 00406 00407 00408 00409 00410 00411 00412 00413 00414 00415 00416 00417 00418 00419 00420 00421 00422 00423 00424 00425 00426 00427 00428 00429 00430 00431 00432 00433 00434 00435 00436 00437 00438 00439 00440 00441 00442 00443 MOVPF MOVFP MOVPF ENDM WREG,b+B2 a+B3,WREG WREG,b+B3 ; move to b(B2) ; get byte of a into w ; move to b(B3) ;***************************************************************************** ;***************************************************************************** ; NAME: MOV24 ; ; DESCRIPTION: 24 bit move ; ; ARGUMENTS: a => b ; ; TIMING (cycles): 6 ; MOV24 MACRO MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: MOV16 ; ; DESCRIPTION: 16 bit move ; ; ARGUMENTS: a => b ; ; TIMING (in cycles): 4 ; MOV16 MACRO MOVFP MOVPF MOVFP MOVPF a,b a+B0,WREG WREG,b+B0 a+B1,WREG WREG,b+B1 ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) a,b a+B0,WREG WREG,b+B0 a+B1,WREG WREG,b+B1 a+B2,WREG WREG,b+B2 ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) © 1997 Microchip Technology Inc. DS00532C-page 31 00444 00445 00446 00447 00448 00449 00450 00451 00452 00453 00454 00455 00456 00457 00458 00459 00460 00461 00462 00463 00464 00465 00466 00467 00468 00469 00470 00471 00472 00473 00474 00475 00476 00477 00478 00479 00480 00481 00482 00483 00484 00485 00486 00487 00488 00489 00490 ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: MVPF32 ; ; DESCRIPTION: 32 bit move from P data memory to F data memory ; ; ARGUMENTS: A => B ; ; TIMING (cycles): 4 ; MVPF32 MACRO MOVPF MOVPF MOVPF MOVPF ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: MVPF24 ; ; DESCRIPTION: 24 bit move from P data memory to F data memory ; ; ARGUMENTS: A => B ; ; ; TIMING (cycles): 3 ; MVPF24 MACRO MOVPF MOVPF MOVPF ENDM ;***************************************************************************** A,B A+B0,B+B0 A+B1,B+B1 A+B2,B+B2 ; move A(B0) to B(B0) ; move A(B1) to B(B1) ; move A(B2) to B(B2) A,B A+B0,B+B0 A+B1,B+B1 A+B2,B+B2 A+B3,B+B3 ; ; ; ; move move move move A(B0) A(B1) A(B2) A(B3) to to to to B(B0) B(B1) B(B2) B(B3) AN532 DS00532C-page 32 © 1997 Microchip Technology Inc. AN532 00491 00492 00493 00494 00495 00496 00497 00498 00499 00500 00501 00502 00503 00504 00505 00506 00507 00508 00509 00510 00511 00512 00513 00514 00515 00516 00517 00518 00519 00520 00521 00522 00523 00524 00525 00526 00527 00528 00529 00530 00531 00532 00533 00534 00535 00536 00537 ;***************************************************************************** ; NAME: MVPF16 ; ; DESCRIPTION: 16 bit move from P data memory to F data memory ; ; ARGUMENTS: A => B ; ; TIMING (cycles): 2 ; MVPF16 MACRO MOVPF MOVPF ENDM ;***************************************************************************** A,B A+B0,B+B0 A+B1,B+B1 ; move A(B0) to B(B0) ; move A(B1) to B(B1) ;***************************************************************************** ; NAME: MVFP32 ; ; DESCRIPTION: 32 bit move from F data memory to P data memory ; ; ARGUMENTS: A => B ; ; TIMING (cycles): 4 MVFP32 MACRO MOVFP MOVFP MOVFP MOVFP ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: MVFP24 ; ; DESCRIPTION: 24 bit move from F data memory to P data memory ; ; ARGUMENTS: A => B ; A,B A+B0,B+B0 A+B1,B+B1 A+B2,B+B2 A+B3,B+B3 ; ; ; ; move move move move A(B0) A(B1) A(B2) A(B3) to to to to B(B0) B(B1) B(B2) B(B3) © 1997 Microchip Technology Inc. DS00532C-page 33 00538 00539 00540 00541 00542 00543 00544 00545 00546 00547 00548 00549 00550 00551 00552 00553 00554 00555 00556 00557 00558 00559 00560 00561 00562 00563 00564 00565 00566 00567 00568 00569 00570 00571 00572 00573 00574 00575 00576 00577 00578 00579 00580 00581 00582 00583 00584 ; TIMING (cycles): 3 ; MVFP24 MACRO MOVFP MOVFP MOVFP ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: MVFP16 ; ; DESCRIPTION: 16 bit move from F data memory to P data memory ; ; ARGUMENTS: A => B ; ; TIMING (cycles): 2 ; MVFP16 MACRO MOVFP MOVFP ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: LOADAB ; ; DESCRIPTION: Loads extended math library AARG and BARG ; ; ARGUMENTS: A => AARG ; B => BARG ; ; TIMING (cycles): 4 LOADAB MACRO MOVFP MOVFP MOVFP A,B A+B0,AARG+B0 A+B1,AARG+B1 B+B0,BARG+B0 ; load lo byte of A to AARG ; load hi byte of A to AARG ; load lo byte of B to BARG A,B A+B0,B+B0 A+B1,B+B1 ; move A(B0) to B(B0) ; move A(B1) to B(B1) A,B A+B0,B+B0 A+B1,B+B1 A+B2,B+B2 ; move A(B0) to B(B0) ; move A(B1) to B(B1) ; move A(B2) to B(B2) AN532 DS00532C-page 34 © 1997 Microchip Technology Inc. AN532 00585 00586 00587 00588 00589 00590 00591 00592 00593 00594 00595 00596 00597 00598 00599 00600 00601 00602 00603 00604 00605 00606 00607 00608 00609 00610 00611 00612 00613 00614 00615 00616 00617 00618 00619 00620 00621 00622 00623 00624 00625 00626 00627 00628 00629 00630 00631 MOVFP ENDM B+B1,BARG+B1 ; load hi byte of B to BARG ;***************************************************************************** ;***************************************************************************** ; NAME: ADD32 ; ; DESCRIPTION: 32 bit add ; ; ARGUMENTS: a + b => b ; ; TIMING (cycles): 8 ; ADD32 MACRO MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: ADD24 ; ; DESCRIPTION: 24 bit add ; ; ARGUMENTS: a + b => b ; ; TIMING (cycles): 6 ; ADD24 MACRO MOVFP ADDWF MOVFP ADDWFC a,b a+B0,WREG b+B0, F a+B1,WREG b+B1, F ; ; ; ; get add get add lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a,b a+B0,WREG b+B0, F a+B1,WREG b+B1, F a+B2,WREG b+B2, F a+B3,WREG b+B3, F ; ; ; ; ; ; ; ; get add get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of 4th byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) a into w b, save in b(B3) © 1997 Microchip Technology Inc. DS00532C-page 35 00632 00633 00634 00635 00636 00637 00638 00639 00640 00641 00642 00643 00644 00645 00646 00647 00648 00649 00650 00651 00652 00653 00654 00655 00656 00657 00658 00659 00660 00661 00662 00663 00664 00665 00666 00667 00668 00669 00670 00671 00672 00673 00674 00675 00676 00677 00678 MOVFP ADDWFC ENDM a+B2,WREG b+B2, F ; get 3rd byte of a into w ; add 3rd byte of b, save in b(B2) ;***************************************************************************** ;***************************************************************************** ; NAME: ADD16 ; ; DESCRIPTION: 16 bit add ; ; ARGUMENTS: a + b => b ; ; ; TIMING (cycles): 4 ; ADD16 MACRO MOVFP ADDWF MOVFP ADDWFC ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: SUB32 ; ; DESCRIPTION: 32 bit subtract ; ; ; ARGUMENTS: b - a => b ; ; TIMING (cycles): 8 ; a,b a+B0,WREG b+B0, F a+B1,WREG b+B1, F ; ; ; ; get add get add lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) AN532 SUB32 MACRO MOVFP SUBWF MOVFP SUBWFB MOVFP a,b a+B0,WREG b+B0, F a+B1,WREG b+B1, F a+B2,WREG ; ; ; ; ; get sub get sub get lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w DS00532C-page 36 © 1997 Microchip Technology Inc. AN532 00679 00680 00681 00682 00683 00684 00685 00686 00687 00688 00689 00690 00691 00692 00693 00694 00695 00696 00697 00698 00699 00700 00701 00702 00703 00704 00705 00706 00707 00708 00709 00710 00711 00712 00713 00714 00715 00716 00717 00718 00719 00720 00721 00722 00723 00724 00725 SUBWFB MOVFP SUBWFB ENDM b+B2, F a+B3,WREG b+B3, F ; sub 3rd byte of b, save in b(B2) ; get 4th byte of a into w ; sub 4th byte of b, save in b(B3) ;***************************************************************************** ;***************************************************************************** ; NAME: SUB24 ; ; DESCRIPTION: 24 bit subtract ; ; ARGUMENTS: b - a => b ; ; TIMING (in cycles): 6 ; SUB24 MACRO MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: SUB16 ; ; DESCRIPTION: 16 bit subtract ; ; ARGUMENTS: b - a => b ; ; TIMING (cycles): 4 ; SUB16 MACRO MOVFP SUBWF MOVFP SUBWFB a,b a+B0,WREG b+B0, F a+B1,WREG b+B1, F ; ; ; ; get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a,b a+B0,WREG b+B0, F a+B1,WREG b+B1, F a+B2,WREG b+B2, F ; ; ; ; ; ; get sub get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) © 1997 Microchip Technology Inc. DS00532C-page 37 00726 00727 00728 00729 00730 00731 00732 00733 00734 00735 00736 00737 00738 00739 00740 00741 00742 00743 00744 00745 00746 00747 00748 00749 00750 00751 00752 00753 00754 00755 00756 00757 00758 00759 00760 00761 00762 00763 00764 00765 00766 00767 00768 00769 00770 00771 00772 ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: RLC32 ; ; DESCRIPTION: 32 bit rotate left ; ; ARGUMENTS: 2*a => a ; ; TIMING (cycles): 5 ; RLC32 MACRO BCF RLCF RLCF RLCF RLCF ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: RLC24 ; ; DESCRIPTION: 24 bit rotate left ; ; ARGUMENTS: 2*a => a ; ; TIMING (cycles): 4 ; RLC24 MACRO BCF RLCF RLCF RLCF ENDM ;***************************************************************************** a _carry a+B0, F a+B1, F a+B2, F a _carry a+B0, F a+B1, F a+B2, F a+B3, F AN532 DS00532C-page 38 © 1997 Microchip Technology Inc. AN532 00773 00774 00775 00776 00777 00778 00779 00780 00781 00782 00783 00784 00785 00786 00787 00788 00789 00790 00791 00792 00793 00794 00795 00796 00797 00798 00799 00800 00801 00802 00803 00804 00805 00806 00807 00808 00809 00810 00811 00812 00813 00814 00815 00816 00817 00818 00819 ;***************************************************************************** ; NAME: RLC16 ; ; DESCRIPTION: 16 bit rotate left ; ; ARGUMENTS: 2*a => a ; ; ; TIMING (cycles): 3 ; RLC16 MACRO BCF RLCF RLCF ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: RRC32 ; ; DESCRIPTION: 32 bit rotate right ; ; ARGUMENTS: a/2 => a ; ; TIMING (cycles): 5 ; RRC32 MACRO RLCF RRCF RRCF RRCF RRCF ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: RRC24 ; ; DESCRIPTION: 24 bit rotate right a a+B3,W a+B3, F a+B2, F a+B1, F a+B0, F ; move sign into carry bit a _carry a+B0, F a+B1, F © 1997 Microchip Technology Inc. DS00532C-page 39 00820 00821 00822 00823 00824 00825 00826 00827 00828 00829 00830 00831 00832 00833 00834 00835 00836 00837 00838 00839 00840 00841 00842 00843 00844 00845 00846 00847 00848 00849 00850 00851 00852 00853 00854 00855 00856 00857 00858 00859 00860 00861 00862 00863 00864 00865 00866 ; ; ARGUMENTS: a/2 => a ; ; TIMING (cycles): 4 ; RRC24 MACRO RLCF RRCF RRCF RRCF ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: RRC16 ; ; DESCRIPTION: 16 bit rotate right ; ; ENTRY CONDITIONS: a/2 => a ; ; TIMING (cycles): 3 ; RRC16 MACRO RLCF RRCF RRCF ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: INC24 ; ; DESCRIPTION: 24 bit increment ; ; ARGUMENTS: a+1 => a ; ; TIMING (cycles): 4 ; a a+B1,W a+B1, F a+B0, F ; move sign into carry bit a a+B2,W a+B2, F a+B1, F a+B0, F ; move sign into carry bit AN532 DS00532C-page 40 © 1997 Microchip Technology Inc. AN532 00867 00868 00869 00870 00871 00872 00873 00874 00875 00876 00877 00878 00879 00880 00881 00882 00883 00884 00885 00886 00887 00888 00889 00890 00891 00892 00893 00894 00895 00896 00897 00898 00899 00900 00901 00902 00903 00904 00905 00906 00907 00908 00909 00910 00911 00912 00913 INC24 MACRO CLRF INCF ADDWFC ADDWFC ENDM a WREG, a+B0, a+B1, a+B2, F F F F ;***************************************************************************** ;***************************************************************************** ; NAME: INC16 ; ; DESCRIPTION: 16 bit increment ; ; ARGUMENTS: a+1 => a ; ; TIMING (cycles): 3 ; INC16 MACRO CLRF INCF ADDWFC ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: DEC24 ; ; DESCRIPTION: Decrement A 24 Bit Number ; ; ARGUMENTS: a-1 => a ; ; TIMING (cycles): 4 ; DEC24 MACRO CLRF DECF SUBWFB SUBWFB a WREG, a+B0, a+B1, a+B2, F F F F a WREG, F a+B0, F a+B1, F © 1997 Microchip Technology Inc. DS00532C-page 41 00914 00915 00916 00917 00918 00919 00920 00921 00922 00923 00924 00925 00926 00927 00928 00929 00930 00931 00932 00933 00934 00935 00936 00937 00938 00939 00940 00941 00942 00943 00944 00945 00946 00947 00948 00949 00950 00951 00952 00953 00954 00955 00956 00957 00958 00959 00960 ENDM ;***************************************************************************** ;***************************************************************************** ; DESCRIPTION: Decrement A 16 Bit Number ; ; ARGUMENTS: a-1 => a ; ; TIMING (cycles): 3 ; DEC16 MACRO CLRF DECF SUBWFB ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: NEG32 ; ; DESCRIPTION: 32 bit negate ; ; ARGUMENTS: -A => A ; ; TIMING (cycles): 9 ; NEG32 MACRO COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC ENDM A A+B0, A+B1, A+B2, A+B3, WREG, A+B0, A+B1, A+B2, A+B3, F F F F F F F F F a WREG, F a+B0, F a+B1, F AN532 DS00532C-page 42 © 1997 Microchip Technology Inc. AN532 00961 00962 00963 00964 00965 00966 00967 00968 00969 00970 00971 00972 00973 00974 00975 00976 00977 00978 00979 00980 00981 00982 00983 00984 00985 00986 00987 00988 00989 00990 00991 00992 00993 00994 00995 00996 00997 00998 00999 01000 01001 01002 01003 01004 01005 01006 01007 ;***************************************************************************** ;***************************************************************************** ; NAME: NEG24 ; ; DESCRIPTION: 24 bit negate ; ; ARGUMENTS: -A => A ; ; TIMING (cycles): 7 ; NEG24 MACRO COMF COMF COMF CLRF INCF ADDWFC ADDWFC ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: NEG16 ; ; DESCRIPTION: 16 bit negate ; ; ARGUMENTS: -A => A ; ; TIMING (cycles): 5 ; NEG16 MACRO COMF COMF CLRF INCF ADDWFC ENDM ;***************************************************************************** A A+B0, A+B1, WREG, A+B0, A+B1, F F F F F A A+B0, A+B1, A+B2, WREG, A+B0, A+B1, A+B2, F F F F F F F © 1997 Microchip Technology Inc. DS00532C-page 43 01008 01009 01010 01011 01012 01013 01014 01015 01016 01017 01018 01019 01020 01021 01022 01023 01024 01025 01026 01027 01028 01029 01030 01031 01032 01033 01034 01035 01036 01037 01038 01039 01040 01041 01042 01043 01044 01045 01046 01047 01048 01049 01050 01051 01052 01053 01054 ;***************************************************************************** ; NAME: AUTONO ; ; DESCRIPTION: Sets no auto increment or decrement ; ; TIMING (cycles): 4 AUTONO MACRO BSF BSF BSF BSF ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: AUTOINC ; ; DESCRIPTION: Set auto increment ; ; TIMING (cycles): 4 ; AUTOINC MACRO BSF BCF BSF BCF ENDM ;***************************************************************************** ;***************************************************************************** ; NAME: AUTODEC ; ; DESCRIPTION: Sets auto decrement ; ; TIMING (cycles): 4 ; AUTODEC MACRO _fs0 _fs1 _fs2 _fs3 _fs0 _fs1 _fs2 _fs3 AN532 DS00532C-page 44 © 1997 Microchip Technology Inc. AN532 01055 01056 01057 01058 01059 01060 01061 01062 01063 01064 01065 01066 01067 01068 01069 01070 01071 01072 01073 01074 01075 01076 01077 01078 01079 01080 01081 01082 01083 01084 01085 01086 01087 01088 01089 01090 01091 01092 01093 01094 01095 01096 01097 01098 01099 01100 01101 BCF BCF BCF BCF ENDM _fs0 _fs1 _fs2 _fs3 ;***************************************************************************** ;***************************************************************************** ; NAME: TFSZ32 ; ; DESCRIPTION: 32 bit test and skip if zero ; ; TIMING (cycles): 6 ; TFSZ32 MACRO MOVFP IORWF IORWF IORWF TSTFSZ ENDM A A+B0,WREG A+B1,W A+B2,W A+B3,W WREG ;***************************************************************************** ;***************************************************************************** ; NAME: TFSZ24 ; ; DESCRIPTION: 24 bit test and skip if zero ; ; TIMING (cycles): 5 TFSZ24 MACRO MOVFP IORWF IORWF TSTFSZ ENDM A A+B0,WREG A+B1,W A+B2,W WREG ;***************************************************************************** ;***************************************************************************** © 1997 Microchip Technology Inc. 00000018 0000001C 00000018 0000001C 00000020 00000023 00000026 00000028 0000002A 0000002C 0000002D 0000002F DS00532C-page 45 00000031 00000034 00000037 0000003A 0000003C 00000040 01102 01103 01104 01105 01106 01107 01108 01109 01110 01111 01112 01113 01114 01115 01116 00016 00017 00018 00019 00020 00021 00022 00023 00024 00025 00026 00027 00028 00029 00030 00031 00032 00033 00034 00035 00036 00037 00038 00039 00040 00041 00042 00043 00044 00045 00046 00047 ; NAME: TFSZ16 ; ; DESCRIPTION: 16 bit test and skip if zero ; ; TIMING (cycles): 4 ; TFSZ16 MACRO MOVFP IORWF TSTFSZ ENDM A A+B0,WREG A+B1,W WREG ;***************************************************************************** ;***************************************************************************** ; global variables ; CBLOCK 0x18 DPX,DPX1,DPX2,DPX3 ; arithmetic accumulator AARG,AARG1,BARG,BARG1 ; multiply arguments ENDC CBLOCK 0x18 TMP,TMP1,TMP2,TMP3 MOVTMP,MOVTMP1,MOVTMP2,MOVTMP3 ENDC CBLOCK 0x20 VL,VL1,VL2 AL,AL1,AL2 KP,KP1 KV,KV1 KI,KI1 IM FV,FV1 FA,FA1 VALBUF,VALBUF1,VALBUF2 CVALBUF,CVALBUF1,CVALBUF2 DVALBUF,DVALBUF1,DVALBUF2 ISRBSR,ISRWREG CMDCHAR,CMDTEMP,CMDPTRH,CMDPTRL PARTEMP,PARLEN,PARPTR ; temporary variables ; move temporary storage ; velocity limit ; acceleration limit ; ; ; ; ; ; ; ; ; ; ; ; proportional gain velocity gain integral gain integrator mode velocity feedforward acceleration feedforward AN532 iovalue buffer iovalue buffer iovalue buffer isr save storage command interface variables parameter variables DS00532C-page 46 © 1997 Microchip Technology Inc. AN532 00000043 00000046 00000049 0000004C 0000004F 00000051 00000054 00000056 00000058 0000005B 0000005E 00000062 00000066 0000006A 0000006C 0000006D 0000006F 00000071 00000073 00000074 00000075 00000078 0000007A 0000007C 0000007F 00000082 00000083 00000087 0000008B 0000008F 00000095 00000096 00000097 00000098 00000099 0000009A 0000009B 00048 00049 00050 00051 00052 00053 00054 00055 00056 00057 00058 00059 00060 00061 00062 00063 00064 00065 00066 00067 00068 00069 00070 00071 00072 00073 00074 00075 00076 00077 00078 00079 00080 00081 00082 00083 00084 00085 00086 00087 00088 00089 00090 00091 00092 00093 00094 CPOSITION,CPOSITION1,CPOSITION2 CVELOCITY,CVELOCITY1,CVELOCITY2 CMPOSITION,CMPOSITION1,CMPOSITION2 CMVELOCITY,CMVELOCITY1,CMVELOCITY2 STRVALH,STRVALL HEXVAL,HEXTMP,HEXSTAT OPOSITION,OPOSITION1 OPOSITION2,OPOSITION3 POSITION,POSITION1,POSITION2 VELOCITY,VELOCITY1,VELOCITY2 ; ; ; ; shutter shutter shutter shutter commanded position commanded velocity measured position measured velocity ; string io variables ; hex io variables ; original commanded position ; commanded position ; commanded velocity ; ; ; ; move move half move value value move value time in sample counts NMOVVAL,NMOVVAL1,NMOVVAL2,NMOVVAL3 MOVVAL,MOVVAL1,MOVVAL2,MOVVAL3 HMOVVAL,HMOVVAL1,HMOVVAL2,HMOVVAL3 MOVTIME,MOVTIME1 MOVSIGN T1,T11 T2,T21 TAU,TAU1 NMODE MODE MPOSITION,MPOSITION1,MPOSITION2 MVELOCITY,MVELOCITY1 MVELOCITY2,MVELOCITY3 POSERROR,POSERROR1,POSERROR2 VELERROR,VELERROR1,VELERROR2 SIGN Y,Y1,Y2,Y3 YPWM,YPWM1,YPWM2,YPWM3 YPWMIN,YPWMIN1,YPWMAX,YPWMAX1 U0,U01,U1,U11,U2,U21 SERVOFLAG MODETYPE EXTSTAT MOVSTAT MOVFLAG SATFLAG INTEGRAL,INTEGRAL1 ; ; ; ; ; ; 0x00 for positive, 0x80 for negative time to maximum velocity time for half the move total move time next move modetype move modetype ; measured position ; measured velocity ; position error ; velocity error ; multiply sign ; Y(k) before pwm conversion ; pwm input ; pwm input limits ; saturated error at successive times ; ; ; ; ; ; ; servoflag = 0 => no servo mode flag(0=position,1=velocity,-1=torque) external status register move status register move flag saturation flag (1=pos,-1=neg) integrator © 1997 Microchip Technology Inc. DS00532C-page 47 0000009D 000000A3 000000A7 000000AB 000000AF 000000B3 000000B7 000000B9 000000BB 000000BF 000000C1 000000C4 000000C5 000000C6 000000C8 000000CA 0000 0000 C021 0020 0020 C070 00095 00096 00097 00098 00099 00100 00101 00102 00103 00104 00105 00106 00107 00108 00109 00110 00111 00112 00113 00114 00115 00116 00117 00118 00119 00120 00121 00122 00123 00124 00125 00126 00127 00128 00129 00130 00131 00132 00133 00134 00135 00136 00137 00138 00139 00140 00141 C0,C01,C1,C11,C2,C21 DECVAL,DECSTAT,DECTMP,DECSIGN ; difference equation coefficients ; decimal io variables ; ; ; ; commanded commanded commanded commanded acceleration = +-AL, 0 velocity = +-VL, 0 position buffer velocity buffer A,A1,A2,A3 V,V1,V2,V3 MOVPBUF,MOVPBUF1,MOVPBUF2,MOVPBUF3 MOVVBUF,MOVVBUF1,MOVVBUF2,MOVVBUF3 UPCOUNT,UPCOUNT1 DOWNCOUNT,DOWNCOUNT1 MOVDEL,MOVDEL1,MOVDEL2,MOVDEL3 PH2FLAT,PH2FLAT1 INDEXPOS,INDEXPOS1,INDEXPOS2 SHIFTNUM ; running up counter ; running down counter ; move discretization delta ; phase 2 flat iteration counter ; position at last index pulse ; # of bit shifts from middle 16 ;************************************************************************** ; For PICMASTER Debug/servo tuning Purposes Only CAPFLAG CAPCOUNT,CAPCOUNT1 CAPTMP,CAPTMP1 ; trace capture flag ; PICMASTER trace capture counter ; trace capture temporary storage ;************************************************************************** ZERO,ONE ENDC ;***************************************************************************** ; constants ORG GOTO ORG GOTO 0x0 Startup 0x20 InterruptPoll ; reset vector ; startup vector ; interrupt vector AN532 ;***************************************************************************** ; NAME: Startup ; ; DESCRIPTION: This routine is called on the hardware reset or when the ; program wishes to restore initial conditions. Initiali; zation of run-time constants takes place here. DS00532C-page 48 © 1997 Microchip Technology Inc. AN532 0021 0021 8406 0022 0023 0024 0025 8404 8504 8604 8704 0026 B018 0027 4A01 0028 0028 2900 0029 1F01 002A C028 002B 15CB 002C B803 002D B03F 002E 770A 002F B07F 0030 720A 0031 730A 0032 B0C0 0033 700A 0034 710A 0035 B006 0036 760A 0037 B802 0038 B0FF 0039 740A 003A B00F 00142 00143 00144 00145 00146 00147 00148 00149 00150 00151 M M M M M M 00152 00153 00154 00155 00156 00157 00158 00159 00160 00161 00162 00163 00164 00165 00166 00167 00168 00169 00170 00171 00172 00173 00174 00175 00176 00177 00178 00179 00180 00181 00182 ; ; RETURNS: restart to safe and initial state ; ; STACK UTILIZATION: none ; TIMING (in cycles): X Startup BSF AUTONO BSF BSF BSF BSF MOVLW MOVPF memloop CLRF INCFSZ GOTO INCF MOVLB MOVLW MOVFP MOVLW MOVFP MOVFP MOVLW MOVFP MOVFP MOVLW MOVFP MOVLB MOVLW MOVFP MOVLW INDF0, F FSR0, F memloop ONE, F BANK3 TCON2_INIT WREG,TCON2 PW1DCH_INIT WREG,PW1DCH WREG,PW2DCH PW1DCL_INIT WREG,PW1DCL WREG,PW2DCL TCON1_INIT WREG,TCON1 BANK2 PR1_INIT WREG,PR1 PR2_INIT ; set organization of timers ; BANK3 initialization _glintd ; disable all interrupts ; no auto increment or decrement _fs0 _fs1 _fs2 _fs3 0x18 WREG,FSR0 ; clear all memory locations [18,FF] ; set duty cycle to midpoint ; BANK2 initialization ; initialize timer1 period © 1997 Microchip Technology Inc. DS00532C-page 49 003B 750A 003C B800 003D B080 003E 650A 003F B090 0040 730A 0041 B020 0042 750A 0043 B019 0044 770A 0045 B0F3 0046 710A 0047 B801 0048 B008 0049 4A27 004A B050 004B 4A29 004C B004 004D 4A2B 004E B001 004F 4A24 0050 B0F0 0051 4A21 0052 82C4 0053 5288 0054 0055 0056 0057 0058 0059 B080 4A8D B0FE 4A8E B040 4A8B 00183 00184 00185 00186 00187 00188 00189 00190 00191 00192 00193 00194 00195 00196 00197 00198 00199 00200 00201 00202 00203 00204 00205 00206 00207 00208 00209 00210 00211 00212 00213 00214 00215 00216 00217 00218 00219 00220 00221 00222 00223 00224 00225 00226 00227 00228 00229 MOVFP WREG,PR2 ; initialize timer2 period MOVLB MOVLW MOVFP MOVLW MOVFP MOVLW MOVFP MOVLW MOVFP MOVLW MOVFP MOVLB MOVLW MOVPF MOVLW MOVPF MOVLW MOVPF MOVLW MOVPF MOVLW MOVPF BSF MOVPF MOVLW MOVPF MOVLW MOVPF MOVLW MOVPF BANK0 T0STA_INIT WREG,RTCSTA RCSTA_INIT WREG,RCSTA TXSTA_INIT WREG,TXSTA SPBRG_INIT WREG,SPBRG DDRB_INIT WREG,DDRB BANK1 0x08 WREG,KP+B1 0x50 WREG,KV+B1 0x04 WREG,KI+B1 0x01 WREG,AL+B1 0xF0 WREG,VL+B1 SHIFTNUM,2 PW1DCH,YPWM+B1 ; BANK0 initialization ; sets T0 for external input ; set receive status ; set transmit status ; set baud rate ; set port B for whatever ; BANK1 initialization ; initialize some parameters ; proportional gain ; derivative gain ; integral gain ; acceleration limit ; velocity limit ; set shift number AN532 PWMAXL WREG,YPWMAX+B0 PWMAXH WREG,YPWMAX+B1 PWMINL WREG,YPWMIN+B0 ; initialize pwm limits DS00532C-page 50 © 1997 Microchip Technology Inc. AN532 005A B001 005B 4A8C 005C 005D 005E 005F 2916 2907 8517 8307 0060 8C06 0061 B802 0062 0062 290B 0063 290C 0064 2912 0065 2913 0066 B0FF 0067 170A 0068 C067 0069 006A 006B 006C 006D 006E 6A0B 080C 0812 0813 330A C062 006F C086 0070 0070 4F3A 0071 4A3B 0072 B801 0073 E5EC 00230 00231 00232 00233 00234 00235 00236 00237 00238 00239 00240 00241 00242 00243 00244 00245 00246 00247 00248 00249 00250 00251 00252 00253 00254 00255 00256 00257 00258 00259 00260 00261 00262 00263 00264 00265 00266 00267 00268 00269 00270 00271 00272 00273 00274 00275 00276 MOVLW MOVPF CLRF CLRF BSF BSF BCF MOVLB zeroctrs CLRF CLRF CLRF CLRF MOVLW DECFSZ GOTO MOVFP IORWF IORWF IORWF TSTFSZ GOTO GOTO PWMINH WREG,YPWMIN+B1 PIR, F INTSTA, F _tm2ie _peie _glintd BANK2 ; clear flags, set indiviual interrupts ; enable interrupts TMR0L, F TMR0H, F TMR3L, F TMR3H, F 0xFF WREG, F delay TMR0L,WREG TMR0H,W TMR3L,W TMR3H,W WREG zeroctrs PollingLoop ; clear up counter ; clear down counter delay ; motor still moving ;***************************************************************************** ;***************************************************************************** ; NAME: InterruptPoll InterruptPoll MOVPF MOVPF MOVLB CALL BSR,ISRBSR WREG,ISRWREG BANK1 doMPosMVel ; calculate measured position and ; velocity ; save BSR,WREG © 1997 Microchip Technology Inc. DS00532C-page 51 0074 E61D 0075 0076 0077 0078 0079 2298 B501 0499 9F0A E37E 007A 9E98 007B E44F 007C E291 007D 3395 007E E2D8 007F 33C5 0080 E742 0081 B801 0082 2916 0083 6F3A 0084 6A3B 0085 0005 0086 0086 0087 0088 0089 E08D E1AC 31CB C086 00277 00278 00279 00280 00281 00282 00283 00284 00285 00286 00287 00288 00289 00290 00291 00292 00293 00294 00295 00296 00297 00298 00299 00300 00301 00302 00303 00304 00305 00306 00307 00308 00309 00310 00311 00312 00313 00314 00315 00316 00317 00318 00319 00320 00321 00322 00323 CALL RLNCF ANDLW SUBWF BTFSC CALL BTFSC CALL CALL TSTFSZ CALL TSTFSZ CALL MOVLB CLRF MOVFP MOVFP RETFIE doExtstat MOVSTAT,W 0x01 MOVFLAG,W WREG,MSB doPreMove MOVSTAT,BIT6 doMove doError SERVOFLAG doServo CAPFLAG doCaptureRegs BANK1 PIR, F ISRBSR,BSR ISRWREG,WREG ; evaluate external status ; ; ; ; if MOVFLAG=0 and MOVSTAT,BIT7=1 then do premove. This is only executed once at the beginning of each move ; is motion continuing? ; if so, do move ; ; ; ; calculate position and velocity error test servoflag, if 0 then no servo do servo ; for PIC-MASTER Trace Capture, demo purposes ; clear all interrupt request flags ; restore BSR,WREG ;***************************************************************************** ;***************************************************************************** ; NAME: PollingLoop ; ; DESCRIPTION: The actual polling loop called after the board’s ; initialization ; ; ENTRY CONDITIONS: System globals and hardware initialized and the ; interrupt processes started. ; AN532 PollingLoop CALL CALL CPFSEQ GOTO IdleFunction GetChk ONE PollingLoop ; GetChk, is receive buffer full? DS00532C-page 52 © 1997 Microchip Technology Inc. AN532 008A E1A1 008B 4A3C 008C C08F 008D 008D 0004 008E 0002 008F 008F 0090 0091 0092 0093 0094 0094 0095 0096 B066 4A0D B007 4A0E AB3D A93D A23E A93F 0097 6A3D 0098 30CA 0099 C0A4 009A 313C 009B C094 00324 00325 00326 00327 00328 00329 00330 00331 00332 00333 00334 00335 00336 00337 00338 00339 00340 00341 00342 00343 00344 00345 00346 00347 00348 00349 00350 00351 00352 00353 00354 00355 00356 00357 00358 00359 00360 00361 00362 00363 00364 00365 00366 00367 00368 00369 00370 CALL MOVPF GOTO GetChar WREG,CMDCHAR DoCommand ; if so, get character ; put in CMDCHAR ;***************************************************************************** ;***************************************************************************** ; NAME: IdleFunction ; DESCRIPTION: ; ; IdleFunction CLRWDT RETURN ;***************************************************************************** ;***************************************************************************** ; NAME: DoCommand ; ; DESCRIPTION: Search command table for command and execute it. ; DoCommand MOVLW MOVPF MOVLW MOVPF TABLRD tryNextCmd TABLRD TLRD TABLRD MOVFP CPFSLT GOTO CPFSEQ GOTO LOW CMD_TABLE WREG,TBLPTRL HIGH CMD_TABLE WREG,TBLPTRH 1,1,CMDTEMP 0,1,CMDTEMP 1,CMDPTRH 0,1,CMDPTRL CMDTEMP,WREG ZERO noCommand CMDCHAR tryNextCmd ; error if end of table ; read entry from table ; CMD_TABLE LSB ; CMD_TABLE MSB This routine will perform work while doing waits in serial I/O functions. © 1997 Microchip Technology Inc. DS00532C-page 53 009C E1A4 009D 009E 009F 00A0 00A0 633E 623F 0000 E1A4 00A1 B00D 00A2 E1A4 00A3 C086 00A4 00A4 B03F 00A5 C0A0 00A6 00A6 B021 00A7 C0A0 00371 00372 00373 00374 00375 00376 00377 00378 00379 00380 00381 00382 00383 00384 00385 00386 00387 00388 00389 00390 00391 00392 00393 00394 00395 00396 00397 00398 00399 00400 00401 00402 00403 00404 00405 00406 00407 00408 00409 00410 00411 00412 00413 00414 00415 00416 00417 CALL MOVFP MOVFP NOP cmdFinish CALL MOVLW CALL GOTO noCommand MOVLW GOTO PutChar CMDPTRH,PCLATH CMDPTRL,PCL ; echo command ; indirect jump to command routine PutChar CR PutChar PollingLoop ; send response character from ; command routine followed by CR CMD_BAD cmdFinish ; send error character ;***************************************************************************** ;***************************************************************************** ; NAME: do_null ; ; DESCRIPTION: The do nothing command used to determine if the chip is ; working. Initiated by a carriage return. do_null MOVLW GOTO CMD_OK cmdFinish ;************************************************************************** ;************************************************************************** ; NAME: do_move ; DESCRIPTION: ; ; ; ; ; ; ; ; ; ; ARGUMENTS: ; Commands the axis to move to a new position or velocity. Position data is relative, and in encoder counts. Velocity data is absolute, and in encoder counts/sample time multiplied by 256. All moves are performed by the controller such that velocity and acceleration limits set into parameter memory will not be violated. All move commands are kept in a one deep FIFO buffer. The command in the buffer is executed as soon as the currently executed command is complete. AN532 M [800000,7FFFFF] DS00532C-page 54 © 1997 Microchip Technology Inc. AN532 00A8 00A8 E217 00A9 9F98 00AA C0B4 00AB 00AC 00AD 00AE 00AF 00B0 6A31 4A5E 6A32 4A5F 6A33 4A60 00B1 8798 00B2 B021 00B3 C0A0 00B4 00B4 B03F 00B5 C0A0 00B6 00418 00419 00420 00421 00422 00423 00424 00425 00426 00427 00428 00429 00430 00431 00432 00433 M M M M M M M M 00434 00435 00436 00437 00438 00439 00440 00441 00442 00443 00444 00445 00446 00447 00448 00449 00450 00451 00452 00453 00454 00455 00456 do_move #if CALL #else CALL #endif BTFSC GOTO MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF BSF MOVLW GOTO bufoverflow MOVLW GOTO MOVSTAT,BIT7 bufoverflow VALBUF,NMOVVAL VALBUF+B0,WREG WREG,NMOVVAL+B0 VALBUF+B1,WREG WREG,NMOVVAL+B1 VALBUF+B2,WREG WREG,NMOVVAL+B2 MOVSTAT,BIT7 CMD_OK cmdFinish ; test if buffer available GetVal DECIO GetDecVal ; if so, accept value into NMOVVAL and ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; set buffer full flag CMD_BAD cmdFinish ; else, return error ;************************************************************************** ;************************************************************************** ; NAME: do_mode ; ; DESCRIPTION: An argument of “P” will cause all subsequent move commands ; to be incremental position moves. A “V” argument will cause ; all subsequent moves to be absolute velocity moves. ; ; ARGUMENTS: O [P,V] ; do_mode © 1997 Microchip Technology Inc. DS00532C-page 55 00B6 00B7 00B8 00B9 00BA 00BB 00BC 00BD 00BD 00BE 00BF 00C0 00C1 00C1 00C2 00C3 00C4 00C5 00C6 00C6 00C7 00C8 00C9 00CA 00CB 00CC 00CC 00CD 00CE 00CE 00CF E08D E1AC 31CB C0B6 E1A1 4A50 2996 B050 3150 C0C1 C0CE B056 3150 C0C6 1596 C0CE B054 3150 C0CC 2B96 2995 C0CE B03F C0A0 6A50 E1A4 00D0 B021 00D1 C0A0 00457 00458 00459 00460 00461 00462 00463 00464 00465 00466 00467 00468 00469 00470 00471 00472 00473 00474 00475 00476 00477 00478 00479 00480 00481 00482 00483 00484 00485 00486 00487 00488 00489 00490 00491 00492 00493 00494 00495 00496 00497 00498 00499 00500 00501 00502 00503 CALL CALL CPFSEQ GOTO CALL MOVPF CLRF testP MOVLW CPFSEQ GOTO GOTO testV MOVLW CPFSEQ GOTO INCF GOTO testT MOVLW CPFSEQ GOTO SETF CLRF GOTO modeerror MOVLW GOTO modeok MOVFP CALL MOVLW GOTO IdleFunction GetChk ONE do_mode GetChar WREG,STRVALL MODETYPE, F ‘P’ STRVALL testV modeok ‘V’ STRVALL testT MODETYPE, F modeok ‘T’ STRVALL modeerror MODETYPE, F SERVOFLAG, F modeok CMD_BAD cmdFinish STRVALL,WREG PutChar CMD_OK cmdFinish ; get single character loop ; MODETYPE=0 for position moves ; position moves for type P ; velocity moves for type V ; MODETYPE=1 for velocity moves ; TORQUE Moves for type ‘T’ ; MODETYPE=-1 for torque moves ; disable servo ; mode error ; echo type character ;***************************************************************************** ;***************************************************************************** ; NAME: do_setparameter ; ; DESCRIPTION: Sets controller parameters to the value given. ; ; Parameter # Range ; ; VL=velocity limit 0 [0,7FFFFF] ; AL=acceleration limit 1 [0,7FFFFF] ; AN532 DS00532C-page 56 00D2 00D2 E253 00D3 B008 00D4 3031 00D5 C0F7 00D6 00D7 00D8 00D9 B089 4A0D B007 4A0E 00DA AB40 00DB 00DB A240 00DC A941 00DD A942 00DE B008 00DF 3040 00E0 C0F7 00E1 6A40 00E2 3131 00E3 C0DB 00E4 6A42 00E5 690A © 1997 Microchip Technology Inc. 00E6 E217 AN532 00504 00505 00506 00507 00508 00509 00510 00511 00512 00513 00514 00515 00516 00517 00518 00519 00520 00521 00522 00523 00524 00525 00526 00527 00528 00529 00530 00531 00532 00533 00534 00535 00536 00537 00538 00539 00540 00541 00542 00543 00544 00545 00546 00547 00548 00549 00550 ; KP=proportional gain 2 ; KP=velocity gain 3 ; KP=integral gain 4 ; ; IM=integrator mode 5 ; ; FV=velocity FF 6 ; FA=acceleration FF 7 ; ; ; ARGUMENTS: S [0,FF] [800000,7FFFFF] ; do_setparameter CALL MOVLW CPFSLT GOTO MOVLW MOVPF MOVLW MOVPF TABLRD setNextPar TLRD TABLRD TABLRD MOVLW CPFSLT GOTO MOVFP CPFSEQ GOTO MOVFP MOVFP #if CALL GetPar NUMPAR VALBUF+B0 Serror LOW PAR_TABLE WREG,TBLPTRL HIGH PAR_TABLE WREG,TBLPTRH 1,1,PARTEMP [8000,7FFF] [8000,7FFF] [8000,7FFF] [0,3] [8000,7FFF] : Not Implemented [8000,7FFF] : Not Implemented ; get parameter number ; check if in range [0,NUMPAR] ; PAR_TABLE LSB ; PAR_TABLE MSB 1,PARTEMP 0,1,PARLEN 0,1,PARPTR NUMPAR PARTEMP Serror PARTEMP,WREG VALBUF+B0 setNextPar PARPTR,WREG WREG,FSR1 DECIO GetDecVal ; read entry from table ; error if end of table ; pointer to parameter in FSR1 ; get new value in VALBUF © 1997 Microchip Technology Inc. DS00532C-page 57 00E7 B031 00E8 610A 00E9 00EA 00EB 00EC 00ED 00ED 00EE 00EF 00F0 8404 8D04 8604 8F04 6800 0741 3341 C0ED 00F1 00F2 00F3 00F4 8404 8504 8604 8704 00F5 B021 00F6 C0A0 00F7 00F7 B03F 00F8 C0A0 00551 00552 00553 00554 00555 00556 00557 00558 00559 00560 M M M M M M 00561 00562 00563 00564 00565 00566 00567 M M M M M M 00568 00569 00570 00571 00572 00573 00574 00575 00576 00577 00578 00579 00580 00581 00582 00583 00584 00585 #else CALL #endif MOVLW VALBUF MOVFP WREG,FSR0 AUTOINC BSF BCF BSF BCF setGetMore MOVFP DECF TSTFSZ GOTO AUTONO BSF BSF BSF BSF _fs0 _fs1 _fs2 _fs3 _fs0 _fs1 _fs2 _fs3 ; pointer to VALBUF in FSR0 ; set autoincrement GetVal INDF0,INDF1 PARLEN, F PARLEN setGetMore ; move new value to parameter ; no autoincrement MOVLW GOTO Serror MOVLW GOTO CMD_OK cmdFinish CMD_BAD cmdFinish ;***************************************************************************** AN532 ;***************************************************************************** ; NAME: do_readparameter ; ; DESCRIPTION: Returns the present value of a parameter. ; ; ARGUMENTS: R [0,FF] ; DS00532C-page 58 © 1997 Microchip Technology Inc. AN532 00F9 00F9 E253 00FA B008 00FB 3031 00FC C121 00FD 00FE 00FF 0100 B089 4A0D B007 4A0E 0101 AB40 0102 0102 A240 0103 A941 0104 A942 0105 B008 0106 3040 0107 C121 0108 6A40 0109 3131 010A C102 010B 6A42 010C 690A 010D B031 010E 610A 010F 0110 0111 0112 8404 8D04 8604 8F04 0113 2931 0114 2932 0115 2933 00586 ; RETURNS: The present value of the requested parameter is returned. 00587 00588 do_readparameter 00589 00590 CALL GetPar ; get parameter number 00591 00592 MOVLW NUMPAR ; check if in range [0,NUMPAR] 00593 CPFSLT VALBUF+B0 00594 GOTO Rerror 00595 00596 MOVLW LOW PAR_TABLE ; PAR_TABLE LSB 00597 MOVPF WREG,TBLPTRL 00598 MOVLW HIGH PAR_TABLE ; PAR_TABLE MSB 00599 MOVPF WREG,TBLPTRH 00600 00601 TABLRD 1,1,PARTEMP 00602 00603 readNextPar 00604 TLRD 1,PARTEMP ; read entry from table 00605 TABLRD 0,1,PARLEN 00606 TABLRD 0,1,PARPTR 00607 00608 MOVLW NUMPAR ; error if end of table 00609 CPFSLT PARTEMP 00610 GOTO Rerror 00611 00612 MOVFP PARTEMP,WREG 00613 CPFSEQ VALBUF+B0 00614 GOTO readNextPar 00615 00616 MOVFP PARPTR,WREG ; pointer to parameter in FSR1 00617 MOVFP WREG,FSR1 00618 00619 MOVLW VALBUF ; pointer to VALBUF in FSR1 00620 MOVFP WREG,FSR0 00621 AUTOINC ; set autoincrement M M BSF _fs0 M BCF _fs1 M BSF _fs2 M BCF _fs3 M 00622 00623 CLR24 VALBUF ; clear old VALBUF M CLRF VALBUF+B0, F M CLRF VALBUF+B1, F M CLRF VALBUF+B2, F © 1997 Microchip Technology Inc. DS00532C-page 59 0116 0116 0117 0118 0119 6008 0741 3341 C116 011A 011B 011C 011D 8404 8504 8604 8704 011E E260 011F B021 0120 C0A0 0121 0121 B03F 0122 C0A0 M 00624 00625 00626 00627 00628 00629 00630 M M M M M M 00631 00632 00633 00634 00635 00636 00637 00638 00639 00640 00641 00642 00643 00644 00645 00646 00647 00648 00649 00650 00651 00652 00653 00654 00655 00656 00657 00658 00659 00660 00661 00662 00663 readGetMore MOVFP DECF TSTFSZ GOTO AUTONO BSF BSF BSF BSF INDF1,INDF0 PARLEN, F PARLEN readGetMore ; read parameter into VALBUF ; no autoincrement _fs0 _fs1 _fs2 _fs3 #if CALL #else CALL #endif MOVLW GOTO Rerror MOVLW GOTO DECIO PutDecVal ; send parameter value PutVal CMD_OK cmdFinish CMD_BAD cmdFinish ;***************************************************************************** ;***************************************************************************** ; NAME: do_shutter ; ; DESCRIPTION: Returns the time (in sample time counts [0,FFFF]) since the ; start of the present move and captures the commanded and ; measured values of position and velocity at the time of the ; command. ; ; ; ARGUMENTS: C ; ; RETURNS: The time since the start of the present move is returned. AN532 DS00532C-page 60 © 1997 Microchip Technology Inc. AN532 0123 0123 8406 0124 0125 0126 0127 0128 0129 6A58 4A43 6A59 4A44 6A5A 4A45 012A 012B 012C 012D 012E 012F 6A5B 4A46 6A5C 4A47 6A5D 4A48 0130 0131 0132 0133 0134 0135 6A75 4A49 6A76 4A4A 6A77 4A4B 0136 0137 0138 0139 013A 013B 6A78 4A4C 6A79 4A4D 6A7A 4A4E 013C 2933 013D 6A6A 00664 ; 00665 00666 do_shutter 00667 00668 BSF 00669 00670 MOV24 M M MOVFP M MOVPF M MOVFP M MOVPF M MOVFP M MOVPF M 00671 MOV24 M M MOVFP M MOVPF M MOVFP M MOVPF M MOVFP M MOVPF M 00672 MOV24 M M MOVFP M MOVPF M MOVFP M MOVPF M MOVFP M MOVPF M 00673 MOV24 M M MOVFP M MOVPF M MOVFP M MOVPF M MOVFP M MOVPF M 00674 00675 CLRF 00676 MOV16 M M MOVFP _glintd POSITION,CPOSITION POSITION+B0,WREG WREG,CPOSITION+B0 POSITION+B1,WREG WREG,CPOSITION+B1 POSITION+B2,WREG WREG,CPOSITION+B2 VELOCITY,CVELOCITY VELOCITY+B0,WREG WREG,CVELOCITY+B0 VELOCITY+B1,WREG WREG,CVELOCITY+B1 VELOCITY+B2,WREG WREG,CVELOCITY+B2 MPOSITION,CMPOSITION MPOSITION+B0,WREG WREG,CMPOSITION+B0 MPOSITION+B1,WREG WREG,CMPOSITION+B1 MPOSITION+B2,WREG WREG,CMPOSITION+B2 MVELOCITY,CMVELOCITY MVELOCITY+B0,WREG WREG,CMVELOCITY+B0 MVELOCITY+B1,WREG WREG,CMVELOCITY+B1 MVELOCITY+B2,WREG WREG,CMVELOCITY+B2 ; disable all interrupts ; capture commanded position ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; capture commanded velocity ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; capture measured position ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; capture measured velocity ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) VALBUF+B2, F MOVTIME,VALBUF MOVTIME+B0,WREG ; capture move time, move to VALBUF ; get byte of a into w © 1997 Microchip Technology Inc. DS00532C-page 61 013E 4A31 013F 6A6B 0140 4A32 0141 8C06 0142 E260 0143 B021 0144 C0A0 0145 0145 0146 0147 0148 0149 014A 6A43 4A31 6A44 4A32 6A45 4A33 M M M M 00677 00678 00679 00680 00681 00682 00683 00684 00685 00686 00687 00688 00689 00690 00691 00692 00693 00694 00695 00696 00697 00698 00699 00700 00701 00702 00703 00704 00705 00706 00707 00708 M M M M M M M M 00709 00710 00711 MOVPF MOVFP MOVPF WREG,VALBUF+B0 MOVTIME+B1,WREG WREG,VALBUF+B1 ; move to b(B0) ; get byte of a into w ; move to b(B1) BCF #if CALL #else CALL #endif MOVLW GOTO _glintd DECIO PutDecVal ; enable all interrupts PutVal CMD_OK cmdFinish ;***************************************************************************** ;***************************************************************************** ; NAME: do_readcomposition ; ; DESCRIPTION: Returns the commanded position count which was captured ; during the last shutter command. ; ; ARGUMENTS: P ; ; RETURNS: The last captured position count is returned. [800000,7FFFFF] ; do_readcomposition MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CPOSITION,VALBUF CPOSITION+B0,WREG WREG,VALBUF+B0 CPOSITION+B1,WREG WREG,VALBUF+B1 CPOSITION+B2,WREG WREG,VALBUF+B2 ; move CPOSITION to VALBUF ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) AN532 #if DECIO DS00532C-page 62 © 1997 Microchip Technology Inc. AN532 014B E260 014C B021 014D C0A0 014E 014E 014F 0150 0151 0152 0153 6A46 4A31 6A47 4A32 6A48 4A33 0154 E260 00712 00713 00714 00715 00716 00717 00718 00719 00720 00721 00722 00723 00724 00725 00726 00727 00728 00729 00730 00731 00732 00733 00734 00735 00736 00737 00738 00739 M M M M M M M M 00740 00741 00742 00743 00744 00745 00746 00747 00748 00749 00750 CALL #else CALL #endif MOVLW GOTO PutDecVal PutVal CMD_OK cmdFinish ;***************************************************************************** ;***************************************************************************** ; NAME: do_readcomvelocity ; ; DESCRIPTION: Returns the commanded velocity multiplied by 256 which was ; captured during the last shutter command. ; ; ARGUMENTS: V ; ; RETURNS: The last captured commanded velocity times 256 is returned. ; [800000,7FFFFF] ; do_readcomvelocity MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CVELOCITY,VALBUF CVELOCITY+B0,WREG WREG,VALBUF+B0 CVELOCITY+B1,WREG WREG,VALBUF+B1 CVELOCITY+B2,WREG WREG,VALBUF+B2 ; move commanded velocity to VALBUF ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) #if CALL #else CALL #endif DECIO PutDecVal PutVal © 1997 Microchip Technology Inc. DS00532C-page 63 0155 B021 0156 C0A0 0157 0157 0158 0159 015A 015B 015C 6A49 4A31 6A4A 4A32 6A4B 4A33 015D E260 015E B021 015F C0A0 00751 00752 00753 00754 00755 00756 00757 00758 00759 00760 00761 00762 00763 00764 00765 00766 00767 00768 00769 00770 M M M M M M M M 00771 00772 00773 00774 00775 00776 00777 00778 00779 00780 00781 00782 00783 00784 00785 00786 00787 00788 00789 MOVLW GOTO CMD_OK cmdFinish ;***************************************************************************** ;***************************************************************************** ; NAME: do_readactposition ; ; DESCRIPTION: Returns the measured position count which was captured during ; the last shutter command. ; ; ARGUMENTS: p ; ; RETURNS: The last captured measured position count is returned. ; [800000,7FFFFF] ; do_readactposition MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CMPOSITION,VALBUF CMPOSITION+B0,WREG WREG,VALBUF+B0 CMPOSITION+B1,WREG WREG,VALBUF+B1 CMPOSITION+B2,WREG WREG,VALBUF+B2 ; move measured position to VALBUF ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) #if CALL #else CALL #endif MOVLW GOTO DECIO PutDecVal PutVal CMD_OK cmdFinish AN532 ;***************************************************************************** ;***************************************************************************** ; NAME: do_readactvelocity ; DS00532C-page 64 0160 0160 0161 0162 0163 0164 0165 6A4C 4A31 6A4D 4A32 6A4E 4A33 0166 E260 0167 B021 0168 C0A0 © 1997 Microchip Technology Inc. AN532 00790 00791 00792 00793 00794 00795 00796 00797 00798 00799 00800 00801 M M M M M M M M 00802 00803 00804 00805 00806 00807 00808 00809 00810 00811 00812 00813 00814 00815 00816 00817 00818 00819 00820 00821 00822 00823 00824 00825 00826 00827 00828 ; DESCRIPTION: ; ; ; ARGUMENTS: ; ; RETURNS: ; ; Returns the measured velocity multiplied by 256 which was captured during the last shutter command. v The last captured measured velocity times 256 is returned. [800000,7FFFFF] do_readactvelocity MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CMVELOCITY,VALBUF CMVELOCITY+B0,WREG WREG,VALBUF+B0 CMVELOCITY+B1,WREG WREG,VALBUF+B1 CMVELOCITY+B2,WREG WREG,VALBUF+B2 ; move measured velocity to VALBUF ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) #if CALL #else CALL #endif MOVLW GOTO DECIO PutDecVal PutVal CMD_OK cmdFinish ;***************************************************************************** ;***************************************************************************** ; NAME: do_externalstatus ; ; DESCRIPTION: Returns a two digit hex number which defines the state of ; the bits in the external status register. Issuing this ; command will clear all the bits in the external status ; register unless the event which set the bit is still true. ; ; ; ARGUMENTS: X ; © 1997 Microchip Technology Inc. DS00532C-page 65 0169 0169 016A 016B 016C 016D 8406 6A97 2997 8C06 E1B3 016E B021 016F C0A0 0170 0170 6A98 0171 E1B3 0172 B021 0173 C0A0 00829 00830 00831 00832 00833 00834 00835 00836 00837 00838 00839 00840 00841 00842 00843 00844 00845 00846 00847 00848 00849 00850 00851 00852 00853 00854 00855 00856 00857 00858 00859 00860 00861 00862 00863 00864 00865 00866 00867 00868 00869 00870 00871 00872 00873 00874 00875 ; RETURNS: ; The external status register is returned. do_externalstatus BSF MOVFP CLRF BCF CALL MOVLW GOTO _glintd EXTSTAT,WREG EXTSTAT, F _glintd PutHex CMD_OK cmdFinish ;***************************************************************************** ;***************************************************************************** ; NAME: do_movestatus ; ; DESCRIPTION: Returns a two digit hex number which defines the state of ; the bits in the move status register. Issuing this command ; will clear all the bits in the move status register unless ; the event which set the bit is still true. ; ; ARGUMENTS: Y ; ; RETURNS: The move status register is returned. ; do_movestatus MOVFP CALL MOVLW GOTO MOVSTAT,WREG PutHex CMD_OK cmdFinish ;***************************************************************************** ;***************************************************************************** ; NAME: do_readindposition ; ; DESCRIPTION: Returns the last index position captured in position counts. ; ; ARGUMENTS: I ; ; RETURNS: The last captured index position is returned. AN532 DS00532C-page 66 © 1997 Microchip Technology Inc. AN532 0174 0174 0175 0176 0177 0178 0179 6AC1 4A31 6AC2 4A32 6AC3 4A33 017A E260 017B B021 017C C0A0 017D 017D E217 00876 00877 00878 00879 00880 M M M M M M M M 00881 00882 00883 00884 00885 00886 00887 00888 00889 00890 00891 00892 00893 00894 00895 00896 00897 00898 00899 00900 00901 00902 00903 00904 00905 00906 00907 00908 00909 00910 00911 00912 00913 00914 ; do_readindposition MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF INDEXPOS,VALBUF INDEXPOS+B0,WREG WREG,VALBUF+B0 INDEXPOS+B1,WREG WREG,VALBUF+B1 INDEXPOS+B2,WREG WREG,VALBUF+B2 ; move measured velocity to VALBUF ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) #if CALL #else CALL #endif MOVLW GOTO DECIO PutDecVal PutVal CMD_OK cmdFinish ;***************************************************************************** ;***************************************************************************** ; NAME: do_setposition ; ; DESCRIPTION: Sets the measured and commanded position to the value given. ; This command should not be sent unless the move FIFO buffer is empty. ; ; ARGUMENTS: H [800000,7FFFFF] ; do_setposition #if CALL #else CALL GetVal DECIO GetDecVal © 1997 Microchip Technology Inc. DS00532C-page 67 017E 017F 0180 0181 0182 0183 6A31 4A58 6A32 4A59 6A33 4A5A 0184 0185 0186 0187 0188 0189 6A31 4A75 6A32 4A76 6A33 4A77 018A 018B 018C 018D 2983 2984 2985 2986 018E B021 018F C0A0 0190 0190 B021 0191 E1A4 0192 C021 00915 00916 00917 00918 M M M M M M M M 00919 M M M M M M M M 00920 00921 M M M M M 00922 00923 00924 00925 00926 00927 00928 00929 00930 00931 00932 00933 00934 00935 00936 00937 00938 00939 00940 #endif MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF VALBUF,POSITION VALBUF+B0,WREG WREG,POSITION+B0 VALBUF+B1,WREG WREG,POSITION+B1 VALBUF+B2,WREG WREG,POSITION+B2 VALBUF,MPOSITION VALBUF+B0,WREG WREG,MPOSITION+B0 VALBUF+B1,WREG WREG,MPOSITION+B1 VALBUF+B2,WREG WREG,MPOSITION+B2 ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) CLR32 CLRF CLRF CLRF CLRF Y Y+B0, Y+B1, Y+B2, Y+B3, F F F F MOVLW GOTO CMD_OK cmdFinish ;***************************************************************************** ;***************************************************************************** ; NAME: do_reset ; ; DESCRIPTION: Performs a software reset. ; ; ARGUMENTS: Z ; do_reset MOVLW CALL GOTO CMD_OK PutChar Startup AN532 DS00532C-page 68 © 1997 Microchip Technology Inc. AN532 0193 0193 2995 0194 B021 0195 C0A0 0196 0196 E217 0197 0198 0199 019A 6A31 4AC6 6A32 4AC7 019B 019C 019D 019E 6A31 4AC8 6A32 4AC9 00941 00942 00943 00944 00945 00946 00947 00948 00949 00950 00951 00952 00953 00954 00955 00956 00957 00958 00959 00960 00961 00962 00963 00964 00965 00966 00967 00968 00969 00970 00971 00972 00973 M M M M M M 00974 M M M M M M 00975 ;***************************************************************************** ; NAME: do_stop ; ; DESCRIPTION: Stops servo by clearing SERVOFLAG. ; do_stop CLRF MOVLW GOTO SERVOFLAG, F CMD_OK cmdFinish ;***************************************************************************** ;***************************************************************************** ; NAME: do_capture ; do_capture #if CALL #else CALL #endif MOV16 MOVFP MOVPF MOVFP MOVPF MOV16 MOVFP MOVPF MOVFP MOVPF VALBUF,CAPCOUNT VALBUF+B0,WREG WREG,CAPCOUNT+B0 VALBUF+B1,WREG WREG,CAPCOUNT+B1 VALBUF,CAPTMP VALBUF+B0,WREG WREG,CAPTMP+B0 VALBUF+B1,WREG WREG,CAPTMP+B1 ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) GetVal DECIO GetDecVal © 1997 Microchip Technology Inc. DS00532C-page 69 019F B021 01A0 C0A0 01A1 01A1 B800 01A2 540A 01A3 0002 01A4 01A4 B801 01A5 01A5 9116 01A6 C1A5 01A7 B800 01A8 01A8 9115 01A9 C1A8 01AA 4A16 01AB 0002 00976 00977 00978 00979 00980 00981 00982 00983 00984 00985 00986 00987 00988 00989 00990 00991 00992 00993 00994 00995 00996 00997 00998 00999 01000 01001 01002 01003 01004 01005 01006 01007 01008 01009 01010 01011 01012 01013 01014 01015 01016 01017 01018 01019 01020 01021 01022 MOVLW GOTO CMD_OK cmdFinish ;***************************************************************************** ; NAME: GetChar ; ; DESCRIPTION: Get character from receive buffer. ; GetChar MOVLB MOVPF RETURN ;***************************************************************************** ;***************************************************************************** ; NAME: PutChar ; ; DESCRIPTION: send character out the serial port ; ; ARGUMENTS: WREG contains byte to be transmitted ; PutChar MOVLB bufwait BTFSS GOTO MOVLB shfwait BTFSS GOTO MOVPF RETURN _trmt shfwait WREG,TXREG ; is transmit shift register empty? _tbmt bufwait BANK0 ; set bank0 BANK1 ; set bank1 ; is transmit buffer empty? BANK0 RCREG,WREG ; set bank0 ; receive character ; if so, send character AN532 ;***************************************************************************** ;***************************************************************************** ; NAME: GetChk ; ; DESCRIPTION: Check if character is in receive buffer. DS00532C-page 70 © 1997 Microchip Technology Inc. AN532 01AC 01AC 01AD 01AE 01AF B801 560A B501 0002 01B0 01B0 B130 01B1 E1A4 01B2 0002 01B3 01B3 01B4 01B5 01B6 4A51 1D0A B50F 4A52 01023 01024 01025 01026 01027 01028 01029 01030 01031 01032 01033 01034 01035 01036 01037 01038 01039 01040 01041 01042 01043 01044 01045 01046 01047 01048 01049 01050 01051 01052 01053 01054 01055 01056 01057 01058 01059 01060 01061 01062 01063 01064 01065 01066 01067 01068 01069 ; GetChk MOVLB MOVPF ANDLW RETURN BANK1 PIR,WREG CHARREADY ; set bank1 ; return status in WREG ;***************************************************************************** ;***************************************************************************** ; NAME: PutDec ; ; DESCRIPTION: Converts a hex value [0,F] in WREG to its ASCII equivalent. ; The upper nibble of WREG is assumed to be zero. ; ; ENTRY CONDITIONS: WREG = value to be converted and sent in ASCII decimal ; #if PutDec ADDLW CALL RETURN #endif ;***************************************************************************** ;***************************************************************************** ; NAME: PutHex ; ; DESCRIPTION: Convert the WREG value to ASCII hexadecimal. The output ; format is two digits with the A-F parts in upper case and ; leading zeros. The result is sent out the serial port with ; PutChar. ; ; ENTRY CONDITIONS: WREG = value to be converted and sent in ASCII hex ; PutHex MOVPF SWAPF ANDLW MOVPF WREG,HEXVAL WREG, F 0x0F WREG,HEXTMP 0x30 PutChar ; convert to ASCII DECIO © 1997 Microchip Technology Inc. DS00532C-page 71 01B7 01B8 01B9 01BA 01BB 01BC 01BD 01BE 01BE 01BF 01C0 01C0 01C1 01C2 01C3 01C4 01C5 01C6 01C7 01C8 01C9 01CA 01CB 01CB 01CC 01CD 01CD 2D0A B109 970A C1BE B037 0E52 C1C0 B030 0E52 E1A4 6A51 B50F 4A52 2D0A B109 970A C1CB B037 0E52 C1CD B030 0E52 E1A4 01CE 0002 01CF 01CF AB4F 01D0 01D0 A24F 01D1 A950 01D2 6A4F 01D3 31CA 01070 01071 01072 01073 01074 01075 01076 01077 01078 01079 01080 01081 01082 01083 01084 01085 01086 01087 01088 01089 01090 01091 01092 01093 01094 01095 01096 01097 01098 01099 01100 01101 01102 01103 01104 01105 01106 01107 01108 01109 01110 01111 01112 01113 01114 01115 01116 NEGW ADDLW BTFSS GOTO MOVLW ADDWF GOTO puth20 MOVLW ADDWF puth25 CALL MOVFP ANDLW MOVPF NEGW ADDLW BTFSS GOTO MOVLW ADDWF GOTO putl20 MOVLW ADDWF putl25 CALL RETURN WREG, F 0x09 WREG,MSB puth20 ‘A’-0x0A HEXTMP,W puth25 ‘0’ HEXTMP,W PutChar HEXVAL,WREG 0x0F WREG,HEXTMP WREG, F 0x09 WREG,MSB putl20 ‘A’-0x0A HEXTMP,W putl25 ‘0’ HEXTMP,W PutChar ;***************************************************************************** ;***************************************************************************** ; NAME: PutStr ; ; DESCRIPTION: Sends a character string out the serial port. ; PutStr AN532 TABLRD GetNextPair TLRD TABLRD MOVFP CPFSEQ 1,1,STRVALH 1,STRVALH 0,1,STRVALL STRVALH,WREG ZERO DS00532C-page 72 © 1997 Microchip Technology Inc. AN532 01D4 C1D6 01D5 0002 01D6 01D6 E1A4 01D7 01D8 01D9 01DA 01DB 01DB 6A50 31CA C1DB 0002 E1A4 01DC C1D0 01DD 01DD 01DD 01DE 01DF 01E0 01E1 01E2 01E3 01E4 01E5 01E6 01E7 01E8 01E9 01EA 01EA 01EB 01EC 01ED E08D E1AC 31CB C1DD 2953 E1A1 4A51 E1A4 B00D 0451 330A C1EA C1F4 6A51 B239 970A C1F0 01117 01118 01119 01120 01121 01122 01123 01124 01125 01126 01127 01128 01129 01130 01131 01132 01133 01134 01135 01136 01137 01138 01139 01140 01141 01142 01143 01144 01145 01146 01147 01148 01149 01150 01151 01152 01153 01154 01155 01156 01157 01158 01159 01160 01161 01162 01163 GOTO RETURN putH CALL MOVFP CPFSEQ GOTO RETURN putL CALL GOTO putH PutChar STRVALL,WREG ZERO putL PutChar GetNextPair ;***************************************************************************** ;***************************************************************************** ; NAME: GetHex ; ; DESCRIPTION: Receive an ASCII hex character from the serial port and ; convert to numerical value. ; ; RETURNS: numerical value in HEXVAL GetHex getnxt CALL CALL CPFSEQ GOTO CLRF CALL MOVPF CALL MOVLW SUBWF TSTFSZ GOTO GOTO gth10 MOVFP SUBLW BTFSS GOTO HEXVAL,WREG ‘9’ WREG,MSB gth20 IdleFunction GetChk ONE getnxt HEXSTAT, F GetChar WREG,HEXVAL PutChar CR HEXVAL,W WREG gth10 gthCR © 1997 Microchip Technology Inc. DS00532C-page 73 01EE B009 01EF 0F51 01F0 01F0 01F1 01F2 01F3 B00F 0B51 2953 0002 01F4 01F4 B001 01F5 4A53 01F6 0002 01F7 01F7 01F7 01F8 01F9 01FA E08D E1AC 31CB C1F7 01FB E1A1 01FC 4AA3 01FD E1A4 01FE 01FF 0200 0201 0202 0203 0204 B00D 04A3 30CA C20E B02D 04A3 30CA 01164 01165 01166 01167 01168 01169 01170 01171 01172 01173 01174 01175 01176 01177 01178 01179 01180 01181 01182 01183 01184 01185 01186 01187 01188 01189 01190 01191 01192 01193 01194 01195 01196 01197 01198 01199 01200 01201 01202 01203 01204 01205 01206 01207 01208 01209 01210 MOVLW ADDWF gth20 MOVLW ANDWF CLRF RETURN gthCR MOVLW MOVPF RETURN 0x09 HEXVAL, F 0x0F HEXVAL, F HEXSTAT, F 0x01 WREG,HEXSTAT ;***************************************************************************** ;***************************************************************************** ; NAME: GetDec ; ; DESCRIPTION: Receive an ASCII decimal character from the serial port and ; convert to its numerical value. ; ; ARGUMENTS: numerical value is returned in DECVAL ; #if GetDec getdecnxt CALL CALL CPFSEQ GOTO CALL MOVPF CALL MOVLW SUBWF CPFSLT GOTO MOVLW SUBWF CPFSLT DECIO IdleFunction GetChk ONE getdecnxt GetChar WREG,DECVAL PutChar AN532 CR DECVAL,W ZERO gtdCR MN DECVAL,W ZERO DS00532C-page 74 © 1997 Microchip Technology Inc. AN532 0205 0206 0207 0208 0209 020A 020A 020B 020C 020D 020E 020E 020F 0210 0211 0211 0212 0213 0214 0214 0215 0216 C211 B020 04A3 30CA C214 B00F 0BA3 29A4 0002 B002 4AA4 0002 B001 4AA4 0002 B000 4AA4 0002 01211 01212 01213 01214 01215 01216 01217 01218 01219 01220 01221 01222 01223 01224 01225 01226 01227 01228 01229 01230 01231 01232 01233 01234 01235 01236 01237 01238 01239 01240 01241 01242 01243 01244 01245 01246 01247 01248 01249 01250 01251 01252 01253 01254 01255 01256 01257 GOTO MOVLW SUBWF CPFSLT GOTO gtd09 MOVLW ANDWF CLRF RETURN gtdCR MOVLW MOVPF RETURN gtdMN MOVLW MOVPF RETURN gtdSP MOVLW MOVPF RETURN #endif gtdMN SP DECVAL,W ZERO gtdSP 0x0F DECVAL, F DECSTAT, F DEC_CR WREG,DECSTAT DEC_MN WREG,DECSTAT DEC_SP WREG,DECSTAT ;***************************************************************************** ;***************************************************************************** ; NAME: getval ; ; DESCRIPTION: Get a value [800000,7FFFFF] from the serial port and place ; it in VALBUF. ; #if DECIO #else GetVal CLR24 getnext CALL MOVLW CPFSEQ GOTO RETURN shift SWAPF VALBUF+B2 GetHex 0x01 HEXSTAT shift VALBUF © 1997 Microchip Technology Inc. 0217 0217 2931 0218 2932 0219 2933 DS00532C-page 75 021A 021B 021C 021D 021E E1F7 2BA6 B001 31A4 29A6 01258 01259 01260 01261 01262 01263 01264 01265 01266 01267 01268 01269 01270 01271 01272 01273 01274 01275 01276 01277 01278 01279 01280 01281 01282 01283 01284 01285 01286 01287 01288 01289 01290 01291 01292 01293 01294 M M M M 01295 01296 01297 01298 01299 01300 MOVFP ANDLW MOVPF SWAPF MOVFP ANDLW ADDWF MOVFP ANDLW MOVPF SWAPF MOVFP ANDLW ADDWF MOVFP ANDLW ADDWF MOVPF GOTO #endif VALBUF+B2,WREG 0xF0 WREG,VALBUF+B2 VALBUF+B1 VALBUF+B1,WREG 0x0F VALBUF+B2, F VALBUF+B1,WREG 0xF0 WREG,VALBUF+B1 VALBUF+B0 VALBUF+B0,WREG 0x0F VALBUF+B1 VALBUF+B0,WREG 0xF0 HEXVAL,W WREG,VALBUF+B0 getnext ;***************************************************************************** ;***************************************************************************** ; NAME: GetDecVal ; ; DESCRIPTION: Get a value [-8388608,8388607] from the serial port and ; place it in VALBUF ; ; RETURNS: numerical value is returned in VALBUF #if GetDecVal CLR24 CLRF CLRF CLRF CALL SETF MOVLW CPFSEQ CLRF DECIO VALBUF VALBUF+B0, F VALBUF+B1, F VALBUF+B2, F AN532 GetDec DECSIGN, F DEC_MN DECSTAT DECSIGN, F DS00532C-page 76 © 1997 Microchip Technology Inc. AN532 021F 021F E1F7 0220 0221 0222 0223 0224 B002 31A4 C224 C248 0224 0225 0226 0227 8804 1B31 1B32 1B33 0228 0229 022A 022B 022C 022D 6A31 4A37 6A32 4A38 6A33 4A39 022E 022F 0230 0231 8804 1B31 1B32 1B33 0232 0233 0234 0235 8804 1B31 1B32 1B33 0236 0237 0238 0239 023A 023B 6A37 0F31 6A38 1132 6A39 1133 01301 getdecnext 01302 CALL 01303 01304 MOVLW 01305 CPFSEQ 01306 GOTO 01307 GOTO 01308 mul10 01309 01310 RLC24 M M BCF M RLCF M RLCF M RLCF M 01311 MOV24 M M MOVFP M MOVPF M MOVFP M MOVPF M MOVFP M MOVPF M 01312 RLC24 M M BCF M RLCF M RLCF M RLCF M 01313 RLC24 M M BCF M RLCF M RLCF M RLCF M 01314 ADD24 M M MOVFP M ADDWF M MOVFP M ADDWFC M MOVFP M ADDWFC GetDec DEC_CR DECSTAT mul10 fixsign VALBUF _carry VALBUF+B0, F VALBUF+B1, F VALBUF+B2, F VALBUF,DVALBUF VALBUF+B0,WREG WREG,DVALBUF+B0 VALBUF+B1,WREG WREG,DVALBUF+B1 VALBUF+B2,WREG WREG,DVALBUF+B2 VALBUF _carry VALBUF+B0, F VALBUF+B1, F VALBUF+B2, F VALBUF _carry VALBUF+B0, F VALBUF+B1, F VALBUF+B2, F DVALBUF,VALBUF DVALBUF+B0,WREG VALBUF+B0, F DVALBUF+B1,WREG VALBUF+B1, F DVALBUF+B2,WREG VALBUF+B2, F ; multiply VALBUF by two ; save in DVALBUF ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; VALBUF now multiplied by eight ; VALBUF now multiplied by ten ; ; ; ; ; ; get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) © 1997 Microchip Technology Inc. DS00532C-page 77 023C 2937 023D 2938 023E 2939 023F 6AA3 0240 4A37 0241 0242 0243 0244 0245 0246 0247 0248 0248 0249 024A 6A37 0F31 6A38 1132 6A39 1133 C21F 290A 32A6 0002 024B 024C 024D 024E 024F 0250 0251 1331 1332 1333 290A 1531 1132 1133 0252 0002 0253 M 01315 M M M M 01316 01317 01318 M M M M M M M M 01319 01320 01321 01322 01323 01324 M M M M M M M M M 01325 01326 01327 01328 01329 01330 01331 01332 01333 01334 01335 01336 01337 01338 01339 CLR24 CLRF CLRF CLRF MOVFP MOVPF ADD24 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC GOTO fixsign CLRF CPFSGT RETURN NEG24 COMF COMF COMF CLRF INCF ADDWFC ADDWFC RETURN #endif DVALBUF DVALBUF+B0, F DVALBUF+B1, F DVALBUF+B2, F DECVAL,WREG WREG,DVALBUF+B0 DVALBUF,VALBUF DVALBUF+B0,WREG VALBUF+B0, F DVALBUF+B1,WREG VALBUF+B1, F DVALBUF+B2,WREG VALBUF+B2, F getdecnext WREG, F DECSIGN VALBUF VALBUF+B0, VALBUF+B1, VALBUF+B2, WREG, F VALBUF+B0, VALBUF+B1, VALBUF+B2, F F F F F F ; ; ; ; ; ; get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) ;***************************************************************************** ;***************************************************************************** ; NAME: GetPar ; ; DESCRIPTION: Get a parameter number [0,FF] from the serial port and place ; it in VALBUF+B0. ; GetPar AN532 DS00532C-page 78 © 1997 Microchip Technology Inc. AN532 0253 2931 0254 2932 0255 2933 0256 0257 0258 0259 025A E1DD 6A51 B50F 4A31 1D31 025B E1DD 025C 6A31 025D 0E51 025E 4A31 025F 0002 01340 M M M M 01341 01342 01343 01344 01345 01346 01347 01348 01349 01350 01351 01352 01353 01354 01355 01356 01357 01358 01359 01360 01361 01362 01363 01364 01365 01366 01367 01368 01369 01370 01371 01372 01373 01374 01375 01376 01377 01378 01379 01380 01381 01382 CLR24 CLRF CLRF CLRF VALBUF VALBUF+B0, F VALBUF+B1, F VALBUF+B2, F CALL MOVFP ANDLW MOVPF SWAPF CALL MOVFP ADDWF MOVPF RETURN GetHex HEXVAL,WREG 0x0F WREG,VALBUF+B0 VALBUF+B0, F GetHex VALBUF+B0,WREG HEXVAL,W WREG,VALBUF+B0 ;***************************************************************************** ;***************************************************************************** ; NAME: PutVal ; ; DESCRIPTION: Sends the value in VALBUF [800000,7FFFFF] out the serial port. ; #if #else PutVal MOVFP CALL MOVFP CALL MOVFP CALL RETURN #endif ;***************************************************************************** ;***************************************************************************** VALBUF+B2,WREG PutHex VALBUF+B1,WREG PutHex VALBUF+B0,WREG PutHex DECIO © 1997 Microchip Technology Inc. DS00532C-page 79 0260 0260 9733 0261 C26C 0262 0263 0264 0265 0266 0267 0268 0269 026A 026B 026C 026C 026D 026E 026E 026F 0270 0271 0272 0273 0273 0274 0275 0276 0277 0278 0279 027A 027A 027B 1331 1332 1333 290A 1531 1132 1133 B02D E1A4 C26E B020 E1A4 B09A 4A0D B007 4A0E A937 A037 AB38 A939 2B0A 3137 C27A C28E 1537 2BA3 01383 01384 01385 01386 01387 01388 01389 01390 01391 01392 01393 01394 M M M M M M M M M 01395 01396 01397 01398 01399 01400 01401 01402 01403 01404 01405 01406 01407 01408 01409 01410 01411 01412 01413 01414 01415 01416 01417 01418 01419 01420 ; NAME: ; ; DESCRIPTION: ; #if PutDecVal BTFSS GOTO NEG24 COMF COMF COMF CLRF INCF ADDWFC ADDWFC MOVLW CALL GOTO pdpos MOVLW CALL pddigits MOVLW MOVPF MOVLW MOVPF TABLRD readNextDec TLRD TABLRD TABLRD SETF CPFSEQ GOTO GOTO getdigit INCF SETF PutDecVal Send the value in VALBUF [-8388608,8388607] out the serial port. DECIO VALBUF+B2,MSB pdpos VALBUF VALBUF+B0, VALBUF+B1, VALBUF+B2, WREG, F VALBUF+B0, VALBUF+B1, VALBUF+B2, MN PutChar pddigits SP PutChar F F F F F F LOW DEC_TABLE WREG,TBLPTRL HIGH DEC_TABLE WREG,TBLPTRH 0,1,DVALBUF+B0 0,DVALBUF+B0 1,1,DVALBUF+B1 0,1,DVALBUF+B2 WREG, F DVALBUF+B0 getdigit unitsposition DVALBUF+B0, F DECVAL, F ; DEC_TABLE LSB ; DEC_TABLE MSB ; read entry from table AN532 ; unitsposition if end of table ; restore to power of 10 ; set DECVAL to -1 DS00532C-page 80 © 1997 Microchip Technology Inc. AN532 027C 027C 15A3 027D 027E 027F 0280 0281 0282 6A37 0531 6A38 0332 6A39 0333 0283 9733 0284 C27C 0285 0286 0287 0288 0289 028A 6A37 0F31 6A38 1132 6A39 1133 028B 6AA3 028C E1B0 028D C273 028E 028E 6A31 028F E1B0 0290 0002 0291 01421 01422 01423 M M M M M M M M 01424 01425 01426 01427 M M M M M M M M 01428 01429 01430 01431 01432 01433 01434 01435 01436 01437 01438 01439 01440 01441 01442 01443 01444 01445 01446 01447 01448 01449 01450 01451 inc INCF SUB24 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB BTFSS GOTO ADD24 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP CALL GOTO unitsposition MOVFP CALL RETURN DECVAL, F DVALBUF,VALBUF DVALBUF+B0,WREG VALBUF+B0, F DVALBUF+B1,WREG VALBUF+B1, F DVALBUF+B2,WREG VALBUF+B2, F VALBUF+B2,MSB inc DVALBUF,VALBUF DVALBUF+B0,WREG VALBUF+B0, F DVALBUF+B1,WREG VALBUF+B1, F DVALBUF+B2,WREG VALBUF+B2, F DECVAL,WREG PutDec readNextDec ; if so, correct VALBUF for next digit ; ; ; ; ; ; get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) ; increment DECVAL ; check if in range ; ; ; ; ; ; get sub get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) ; send DECVAL ; get next table entry VALBUF+B0,WREG PutDec ; unit position value now in VALBUF #endif ;***************************************************************************** ;***************************************************************************** ; NAME: doError ; ; DESCRIPTION: Calculates the position and velocity error. ; doError © 1997 Microchip Technology Inc. DS00532C-page 81 0291 0292 0293 0294 0295 0296 6A58 4A7C 6A59 4A7D 6A5A 4A7E 0297 0298 0299 029A 029B 029C 6A75 057C 6A76 037D 6A77 037E 029D 029E 029F 029F 02A0 02A1 02A2 02A3 02A4 02A5 02A6 02A7 02A8 02A9 02AA 02AA 02AB 02AC 02AD 02AE 02AF 02B0 02B1 02B2 02B3 02B4 9F7E C2AA 6A7D B580 097E 290A 327E C2B4 297E B07F 4A7D 2B7C C2B4 6A7D B37F 0B7E 2B0A 307E C2B4 2B7E 297D 877D 297C 01452 M M M M M M M M 01453 M M M M M M M M 01454 01455 01456 01457 ppos 01458 01459 01460 01461 01462 01463 01464 01465 01466 01467 01468 01469 pneg 01470 01471 01472 01473 01474 01475 01476 01477 01478 01479 01480 psatok 01481 01482 MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF SUB24 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB POSITION,POSERROR POSITION+B0,WREG WREG,POSERROR+B0 POSITION+B1,WREG WREG,POSERROR+B1 POSITION+B2,WREG WREG,POSERROR+B2 MPOSITION,POSERROR MPOSITION+B0,WREG POSERROR+B0, F MPOSITION+B1,WREG POSERROR+B1, F MPOSITION+B2,WREG POSERROR+B2, F ; calculate position error ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; ; ; ; ; ; get sub get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) BTFSC GOTO MOVFP ANDLW IORWF CLRF CPFSGT GOTO CLRF MOVLW MOVPF SETF GOTO MOVFP IORLW ANDWF SETF CPFSLT GOTO SETF CLRF BSF CLRF POSERROR+B2,MSB pneg POSERROR+B1,WREG 0x80 POSERROR+B2, F WREG, F POSERROR+B2 psatok POSERROR+B2, F 0x7F WREG,POSERROR+B1 POSERROR, F psatok POSERROR+B1,WREG 0x7F POSERROR+B2, F WREG, F POSERROR+B2 psatok POSERROR+B2, F POSERROR+B1, F POSERROR+B1,MSB POSERROR, F ; saturate error to lowest 16 bits ; clear high byte for debug purposes AN532 ; set high byte to 0xFF for debug purposes MOV24 VELOCITY,VELERROR ; calculate velocity error DS00532C-page 82 © 1997 Microchip Technology Inc. AN532 02B4 02B5 02B6 02B7 02B8 02B9 6A5B 4A7F 6A5C 4A80 6A5D 4A81 02BA 02BB 02BC 02BD 02BE 02BF 6A78 057F 6A79 0380 6A7A 0381 02C0 02C1 02C2 02C2 02C3 02C4 02C5 02C6 02C7 02C8 02C9 02CA 02CB 02CC 02CD 02CD 02CE 02CF 02D0 02D1 02D2 02D3 02D4 02D5 02D6 02D7 02D7 9F81 C2CD 6A80 B580 0981 290A 3281 C2D7 2981 B07F 4A80 2B7F C2D7 6A80 B37F 0B81 2B0A 3081 C2D7 2B81 2980 8780 297F 0002 M M M M M M M M 01483 M M M M M M M M 01484 01485 01486 01487 01488 01489 01490 01491 01492 01493 01494 01495 01496 01497 01498 01499 01500 01501 01502 01503 01504 01505 01506 01507 01508 01509 01510 01511 01512 01513 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF SUB24 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB VELOCITY+B0,WREG WREG,VELERROR+B0 VELOCITY+B1,WREG WREG,VELERROR+B1 VELOCITY+B2,WREG WREG,VELERROR+B2 MVELOCITY,VELERROR MVELOCITY+B0,WREG VELERROR+B0, F MVELOCITY+B1,WREG VELERROR+B1, F MVELOCITY+B2,WREG VELERROR+B2, F ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; ; ; ; ; ; get sub get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) BTFSC GOTO vpos MOVFP ANDLW IORWF CLRF CPFSGT GOTO CLRF MOVLW MOVPF SETF GOTO vneg MOVFP IORLW ANDWF SETF CPFSLT GOTO SETF CLRF BSF CLRF vsatok RETURN VELERROR+B2,MSB vneg VELERROR+B1,WREG 0x80 VELERROR+B2, F WREG, F VELERROR+B2 vsatok VELERROR+B2, F 0x7F WREG,VELERROR+B1 VELERROR, F vsatok VELERROR+B1,WREG 0x7F VELERROR+B2, F WREG, F VELERROR+B2 vsatok VELERROR+B2, F VELERROR+B1, F VELERROR+B1,MSB VELERROR, F ; saturate error to lowest 16 bits ;***************************************************************************** © 1997 Microchip Technology Inc. DS00532C-page 83 02D8 02D8 02D9 02DA 02DB 6A7C 4A8F 6A7D 4A90 02DC 02DD 02DE 02DF 7C8F 7D90 7E26 7F27 02E0 E630 02E1 02E2 02E3 02E4 5883 5984 5A85 5B86 02E5 290A 02E6 329A 02E7 E618 02E8 02E9 02EA 02EB 7C9B 7D9C 7E2A 7F2B 02EC E630 02ED 6A18 01514 01515 01516 01517 01518 01519 01520 01521 01522 M M M M M M 01523 01524 M M M M M M 01525 01526 M M M M M M 01527 01528 01529 01530 01531 01532 M M M M M M 01533 01534 M M ;***************************************************************************** ; NAME: doServo ; ; DESCRIPTION: Performs the servo loop calculations. ; doServo MOV16 MOVFP MOVPF MOVFP MOVPF POSERROR,U0 POSERROR+B0,WREG WREG,U0+B0 POSERROR+B1,WREG WREG,U0+B1 ; save new position error in U0 ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) LOADAB MOVFP MOVFP MOVFP MOVFP CALL MVPF32 MOVPF MOVPF MOVPF MOVPF U0,KP U0+B0,AARG+B0 U0+B1,AARG+B1 KP+B0,BARG+B0 KP+B1,BARG+B1 Dmult DPX,Y DPX+B0,Y+B0 DPX+B1,Y+B1 DPX+B2,Y+B2 DPX+B3,Y+B3 ; compute KP*U0 ; ; ; ; load load load load lo hi lo hi byte byte byte byte of of of of A A B B to to to to AARG AARG BARG BARG ; Y=KP*U0 ; ; ; ; move move move move A(B0) A(B1) A(B2) A(B3) to to to to B(B0) B(B1) B(B2) B(B3) CLRF CPFSGT CALL LOADAB MOVFP MOVFP MOVFP MOVFP CALL ADD32 MOVFP WREG, F SATFLAG doIntegral INTEGRAL,KI INTEGRAL+B0,AARG+B0 INTEGRAL+B1,AARG+B1 KI+B0,BARG+B0 KI+B1,BARG+B1 Dmult DPX,Y DPX+B0,WREG ; if previous output saturated, do ; not accumulate integrator ; compute KI*INTEGRAL ; ; ; ; load load load load lo hi lo hi byte byte byte byte of of of of A A B B to to to to AARG AARG BARG BARG AN532 ; Y=KP*U0+KI*INTEGRAL ; get lowest byte of a into w DS00532C-page 84 © 1997 Microchip Technology Inc. AN532 02EE 02EF 02F0 02F1 02F2 02F3 02F4 0F83 6A19 1184 6A1A 1185 6A1B 1186 02F5 7C8F 02F6 7D90 02F7 02F8 02F9 02FA 6A91 051C 6A92 031D 02FB 7E28 02FC 7F29 02FD E630 02FE 02FF 0300 0301 0302 0303 0304 0305 6A18 0F83 6A19 1184 6A1A 1185 6A1B 1186 0306 0307 0308 0309 030A 290A 32C4 C311 78C4 030A 8804 M ADDWF M MOVFP M ADDWFC M MOVFP M ADDWFC M MOVFP M ADDWFC M 01535 01536 MVFP16 M M MOVFP M MOVFP M 01537 SUB16 M M MOVFP M SUBWF M MOVFP M SUBWFB M 01538 MVFP16 M M MOVFP M MOVFP M 01539 CALL 01540 ADD32 M M MOVFP M ADDWF M MOVFP M ADDWFC M MOVFP M ADDWFC M MOVFP M ADDWFC M 01541 01542 CLRF 01543 CPFSGT 01544 GOTO 01545 MOVFP 01546 grabloop 01547 RLC32 M M BCF Y+B0, F DPX+B1,WREG Y+B1, F DPX+B2,WREG Y+B2, F DPX+B3,WREG Y+B3, F ; ; ; ; ; ; ; add get add get add get add lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of 4th byte of of b, save a into w b, save in a into w b, save in a into w b, save in in b(B0) b(B1) b(B2) b(B3) U0,AARG U0+B0,AARG+B0 U0+B1,AARG+B1 U1,AARG U1+B0,WREG AARG+B0, F U1+B1,WREG AARG+B1, F KV,BARG KV+B0,BARG+B0 KV+B1,BARG+B1 Dmult DPX,Y DPX+B0,WREG Y+B0, F DPX+B1,WREG Y+B1, F DPX+B2,WREG Y+B2, F DPX+B3,WREG Y+B3, F ; compute KV*(U0-U1) ; move A(B0) to B(B0) ; move A(B1) to B(B1) ; ; ; ; get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) ; move A(B0) to B(B0) ; move A(B1) to B(B1) ; Y=KP*U0+KI*INTEGRAL+KV*(U0-U1) ; ; ; ; ; ; ; ; get add get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of 4th byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) a into w b, save in b(B3) WREG, F SHIFTNUM grabok SHIFTNUM,TMP Y _carry © 1997 Microchip Technology Inc. DS00532C-page 85 030B 030C 030D 030E 1B83 1B84 1B85 1B86 030F 1718 0310 C30A 0311 0311 0312 0313 0314 0314 0315 0316 0317 0318 0319 031A 031B 031C 031D 031E 031F 0320 0321 0321 0322 0323 0324 0325 0326 0327 0328 0329 032A 032B 032C 032D 299A 9F86 C321 6A85 B580 0986 290A 3286 C32D 159A 2986 B07F 4A85 2B84 2B83 C32D 6A85 B37F 0B86 2B0A 3086 C32D 2B9A 2B86 2985 8785 2984 2983 032D 6A84 032E 4A87 M M M M M 01548 01549 01550 01551 01552 01553 01554 01555 01556 01557 01558 01559 01560 01561 01562 01563 01564 01565 01566 01567 01568 01569 01570 01571 01572 01573 01574 01575 01576 01577 01578 01579 01580 01581 01582 01583 01584 01585 01586 M M M RLCF RLCF RLCF RLCF DECFSZ GOTO grabok CLRF BTFSC GOTO poss MOVFP ANDLW IORWF CLRF CPFSGT GOTO INCF CLRF MOVLW MOVPF SETF SETF GOTO negs MOVFP IORLW ANDWF SETF CPFSLT GOTO SETF SETF CLRF BSF CLRF CLRF zero6bits MOV24 MOVFP MOVPF Y+B0, Y+B1, Y+B2, Y+B3, F F F F TMP, F grabloop SATFLAG, F Y+B3,MSB negs Y+B2,WREG 0x80 Y+B3, F WREG, F Y+B3 zero6bits SATFLAG, F Y+B3, F 0x7F WREG,Y+B2 Y+B1, F Y+B0, F zero6bits Y+B2,WREG 0x7F Y+B3, F WREG, F Y+B3 zero6bits SATFLAG, F Y+B3, F Y+B2, F Y+B2,MSB Y+B1, F Y+B0, F ; ; ; ; saturate to middle 16 bits, keeping top 10 bits for PW1DCH and PW1DCL check if Y >= 2**23 ; if not, zero 6 bits ; if so, set Y=0x007FFFFF ; clear for debug purposes ; check if Y <= -2**23 ; if not, zero 6 bits ; if so, set Y = 0xFF800000 AN532 Y+B1,YPWM+B0 Y+B1+B0,WREG WREG,YPWM+B0+B0 ; move Y to YPWM and zero 6 bits ; get byte of a into w ; move to b(B0) DS00532C-page 86 © 1997 Microchip Technology Inc. AN532 032F 0330 0331 0332 6A85 4A88 6A86 4A89 0333 0333 B0C0 0334 0B87 0335 0336 0337 0337 0338 0339 033A 033B 033C 9F88 C33E 9697 C344 2987 2988 2989 298A 033D C344 033E 033E 9597 033F C344 0340 0341 0342 0343 0344 0344 0345 0346 0347 2987 2988 2989 298A B07F 4A19 B0C0 4A18 0348 0349 034A 034B 6A18 0F87 6A19 1188 034C 2919 034D B040 034E 4A18 M M M M M 01587 01588 01589 01590 01591 01592 01593 01594 01595 01596 M M M M M 01597 01598 01599 01600 01601 M M M M M 01602 01603 01604 01605 01606 01607 M M M M M M 01608 01609 01610 01611 01612 MOVFP MOVPF MOVFP MOVPF doTorque MOVLW ANDWF BTFSC GOTO tplimit BTFSS GOTO CLR32 CLRF CLRF CLRF CLRF GOTO tmlimit BTFSS GOTO CLR32 CLRF CLRF CLRF CLRF mplimitok MOVLW MOVPF MOVLW MOVPF ADD16 MOVFP ADDWF MOVFP ADDWFC Y+B1+B1,WREG WREG,YPWM+B0+B1 Y+B1+B2,WREG WREG,YPWM+B0+B2 ; ; ; ; get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; entry point for torque mode 0xC0 YPWM+B0, F YPWM+B1,MSB tmlimit EXTSTAT,BIT6 mplimitok YPWM YPWM+B0, F YPWM+B1, F YPWM+B2, F YPWM+B3, F mplimitok EXTSTAT,BIT5 mplimitok YPWM YPWM+B0, F YPWM+B1, F YPWM+B2, F YPWM+B3, F PW1DCH_INIT WREG,TMP+B1 PW1DCL_INIT WREG,TMP+B0 TMP,YPWM TMP+B0,WREG YPWM+B0, F TMP+B1,WREG YPWM+B1, F ; adjustment from bipolar to unipolar ; for 50% duty cycle ; ; ; ; get add get add lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) CLRF MOVLW MOVPF ADD16 TMP+B1, F 0x40 WREG,TMP+B0 TMP,YPWM ; correct by 1 LSB ; add one to bit5 of PW1DCL © 1997 Microchip Technology Inc. DS00532C-page 87 034F 0350 0351 0352 6A18 0F87 6A19 1188 0353 0353 291A 0354 2989 0355 298A 0356 788D 0357 798E 0358 0359 035A 035B 035C 035D 6A87 0518 6A88 0319 6A89 031A 035E 971A 035F C365 0360 0361 0362 0363 0364 0365 0365 0366 0367 6A8D 4A87 6A8E 4A88 C376 291A 2989 298A 0368 788B 0369 798C 036A 6A87 M M MOVFP M ADDWF M MOVFP M ADDWFC M 01613 01614 testmax 01615 CLRF 01616 CLRF 01617 CLRF 01618 MVFP16 M M MOVFP M MOVFP M 01619 SUB24 M M MOVFP M SUBWF M MOVFP M SUBWFB M MOVFP M SUBWFB M 01620 BTFSS 01621 GOTO 01622 MOV16 M M MOVFP M MOVPF M MOVFP M MOVPF M 01623 GOTO 01624 testmin 01625 CLRF 01626 CLRF 01627 CLRF 01628 MVFP16 M M MOVFP M MOVFP M 01629 SUB24 M M MOVFP TMP+B0,WREG YPWM+B0, F TMP+B1,WREG YPWM+B1, F ; ; ; ; get add get add lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) TMP+B2, F YPWM+B2, F YPWM+B3, F YPWMAX,TMP YPWMAX+B0,TMP+B0 YPWMAX+B1,TMP+B1 YPWM,TMP YPWM+B0,WREG TMP+B0, F YPWM+B1,WREG TMP+B1, F YPWM+B2,WREG TMP+B2, F TMP+B2,MSB testmin YPWMAX,YPWM YPWMAX+B0,WREG WREG,YPWM+B0 YPWMAX+B1,WREG WREG,YPWM+B1 limitok TMP+B2, F YPWM+B2, F YPWM+B3, F YPWMIN,TMP YPWMIN+B0,TMP+B0 YPWMIN+B1,TMP+B1 YPWM,TMP YPWM+B0,WREG ; check pwm maximum limit ; LMD18200 must have a minimum pulse ; so duty cycle must not be 0 or 100% ; move A(B0) to B(B0) ; move A(B1) to B(B1) ; ; ; ; ; ; get sub get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) ; saturate to max ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) ; check pwm minimum limit AN532 ; move A(B0) to B(B0) ; move A(B1) to B(B1) ; get lowest byte of a into w DS00532C-page 88 © 1997 Microchip Technology Inc. AN532 036B 036C 036D 036E 036F 0518 6A88 0319 6A89 031A 0370 9F1A 0371 C376 0372 0373 0374 0375 6A8B 4A87 6A8C 4A88 0376 0376 B803 0377 7087 0378 7288 0379 037A 037B 037C 6A8F 4A91 6A90 4A92 037D 0002 037E 037E 037F 0380 0381 0382 0383 6A5E 4A62 6A5F 4A63 6A60 4A64 M M M M M M 01630 01631 01632 M M M M M M 01633 01634 01635 01636 01637 01638 M M M M M M 01639 01640 01641 01642 01643 01644 01645 01646 01647 01648 01649 01650 01651 M M M M M M M SUBWF MOVFP SUBWFB MOVFP SUBWFB BTFSC GOTO MOV16 MOVFP MOVPF MOVFP MOVPF limitok MOVLB MOVFP MOVFP MOV16 MOVFP MOVPF MOVFP MOVPF TMP+B0, F YPWM+B1,WREG TMP+B1, F YPWM+B2,WREG TMP+B2, F TMP+B2,MSB limitok YPWMIN,YPWM YPWMIN+B0,WREG WREG,YPWM+B0 YPWMIN+B1,WREG WREG,YPWM+B1 ; ; ; ; ; sub get sub get sub lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) ; saturate to min ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) BANK3 YPWM+B0,PW1DCL YPWM+B1,PW1DCH U0,U1 U0+B0,WREG WREG,U1+B0 U0+B1,WREG WREG,U1+B1 ; set new duty cycle ; push errors into U(k-1) ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) RETURN ;***************************************************************************** ;***************************************************************************** ; NAME: doPreMove ; ; DESCRIPTION: doPreMove MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF NMOVVAL,MOVVAL NMOVVAL+B0,WREG WREG,MOVVAL+B0 NMOVVAL+B1,WREG WREG,MOVVAL+B1 NMOVVAL+B2,WREG WREG,MOVVAL+B2 ; move buffer to MOVVAL ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) © 1997 Microchip Technology Inc. DS00532C-page 89 0384 0385 0386 0387 0388 8F98 8698 8598 6ACB 4A99 0389 2954 038A 038B 038C 038D 038E 038F 6A58 4A55 6A59 4A56 6A5A 4A57 0390 0391 0392 0393 0394 0395 0396 0397 6A54 4AAF 6A55 4AB0 6A56 4AB1 6A57 4AB2 0398 299A 0399 296A 039A 296B 039B 296D 039C 296E 039D 296F 039E 2970 039F 2971 03A0 2972 M 01652 01653 01654 01655 01656 01657 01658 01659 01660 M M M M M M M M 01661 M M M M M M M M M M 01662 01663 M M M 01664 M M M 01665 M M M 01666 M M M 01667 BCF BSF BSF MOVFP MOVPF MOVSTAT,BIT7 MOVSTAT,BIT6 MOVSTAT,BIT5 ONE,WREG WREG,MOVFLAG ; clear buffer flag ; set motion status flag ; set move in progress flag ; initialize MOVEFLAG to 1 CLRF MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOV32 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CLRF CLR16 CLRF CLRF CLR16 CLRF CLRF CLR16 CLRF CLRF CLR16 CLRF CLRF CLR32 OPOSITION+B0, F POSITION,OPOSITION+B1 POSITION+B0,WREG WREG,OPOSITION+B1+B0 POSITION+B1,WREG WREG,OPOSITION+B1+B1 POSITION+B2,WREG WREG,OPOSITION+B1+B2 OPOSITION,MOVPBUF OPOSITION+B0,WREG WREG,MOVPBUF+B0 OPOSITION+B1,WREG WREG,MOVPBUF+B1 OPOSITION+B2,WREG WREG,MOVPBUF+B2 OPOSITION+B3,WREG WREG,MOVPBUF+B3 SATFLAG, F MOVTIME MOVTIME+B0, F MOVTIME+B1, F T1 T1+B0, F T1+B1, F T2 T2+B0, F T2+B1, F TAU TAU+B0, F TAU+B1, F MOVDEL ; initialize buffers ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; ; ; ; ; ; ; ; get byte of a move to b(B0) get byte of a move to b(B1) get byte of a move to b(B2) get byte of a move to b(B3) into w into w into w into w ; clear move times ; 0 used as flag for no maximum speed AN532 ; clear move discretization error DS00532C-page 90 © 1997 Microchip Technology Inc. AN532 03A1 03A2 03A3 03A4 29BB 29BC 29BD 29BE 03A5 29BF 03A6 29C0 03A7 3396 03A8 C404 03A9 03A9 7862 03AA 7963 03AB 7A64 03AC 971A 03AD C3B5 03AE 03AF 03B0 03B1 03B2 03B3 03B4 03B5 03B5 03B6 03B7 03B8 03B9 1318 1319 131A 290A 1518 1119 111A 291C 291D 291E 801C 811C 03BA 03BB 03BC 03BD 03BE 03BF 6A1C 0518 6A1D 0319 6A1E 031A M M M M M 01668 M M M 01669 01670 01671 01672 pmode 01673 M M M M M 01674 01675 01676 M M M M M M M M M 01677 mvpos 01678 01679 01680 01681 01682 01683 M M M M M M M M 01684 CLRF CLRF CLRF CLRF CLR16 CLRF CLRF MOVDEL+B0, MOVDEL+B1, MOVDEL+B2, MOVDEL+B3, F F F F ; clear phase 2 flat counter PH2FLAT PH2FLAT+B0, F PH2FLAT+B1, F TSTFSZ GOTO MVFP24 MOVFP MOVFP MOVFP BTFSS GOTO NEG24 COMF COMF COMF CLRF INCF ADDWFC ADDWFC MODETYPE vmode MOVVAL,TMP MOVVAL+B0,TMP+B0 MOVVAL+B1,TMP+B1 MOVVAL+B2,TMP+B2 TMP+B2,MSB mvpos TMP TMP+B0, TMP+B1, TMP+B2, WREG, F TMP+B0, TMP+B1, TMP+B2, F F F F F F ; move A(B0) to B(B0) ; move A(B1) to B(B1) ; move A(B2) to B(B2) CLRF CLRF CLRF BSF BSF SUB24 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB MOVTMP+B0, F MOVTMP+B1, F MOVTMP+B2, F MOVTMP+B0,BIT0 MOVTMP+B0,BIT1 MOVTMP,TMP MOVTMP+B0,WREG TMP+B0, F MOVTMP+B1,WREG TMP+B1, F MOVTMP+B2,WREG TMP+B2, F ; calculate abs(MOVVAL) - 3 ; do immediate move if negative ; ; ; ; ; ; get sub get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) © 1997 Microchip Technology Inc. DS00532C-page 91 03C0 03C1 03C2 03C3 03C4 03C5 971A C3CD 2B95 2999 8D98 8E98 03C6 03C7 03C8 03C9 03CA 03CB 6A62 0F58 6A63 1159 6A64 115A 03CC 0002 03CD 03CD 03CE 03CF 03D0 29B3 29B4 29B5 29B6 03D1 6A64 03D2 B580 03D3 4A6C 03D4 29AE 03D5 03D6 03D7 03D8 03D9 03DA 6A20 4AAB 6A21 4AAC 6A22 4AAD 03DB 29AA 03DC 03DD 03DE 03DF 03E0 03E1 6A23 4AA7 6A24 4AA8 6A25 4AA9 01685 BTFSS 01686 GOTO 01687 SETF 01688 CLRF 01689 BCF 01690 BCF 01691 ADD24 M M MOVFP M ADDWF M MOVFP M ADDWFC M MOVFP M ADDWFC M 01692 RETURN 01693 nonzero 01694 CLR32 M CLRF M CLRF M CLRF M CLRF M 01695 01696 MOVFP 01697 ANDLW 01698 MOVPF 01699 01700 CLRF 01701 MOV24 M M MOVFP M MOVPF M MOVFP M MOVPF M MOVFP M MOVPF M 01702 CLRF 01703 MOV24 M M MOVFP M MOVPF M MOVFP M MOVPF M MOVFP M MOVPF TMP+B2,MSB nonzero SERVOFLAG, F MOVFLAG, F MOVSTAT,BIT5 MOVSTAT,BIT6 MOVVAL,POSITION MOVVAL+B0,WREG POSITION+B0, F MOVVAL+B1,WREG POSITION+B1, F MOVVAL+B2,WREG POSITION+B2, F ; ; ; ; ; ; ; check for zero move ; set servoflag to restore servo get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) MOVVBUF MOVVBUF+B0, MOVVBUF+B1, MOVVBUF+B2, MOVVBUF+B3, F F F F MOVVAL+B2,WREG 0x80 WREG,MOVSIGN V+B3, F VL,V VL+B0,WREG WREG,V+B0 VL+B1,WREG WREG,V+B1 VL+B2,WREG WREG,V+B2 A+B3, F AL,A ; move sign (00h=positive,80h=negative) ; create appropriate velocity and ; acceleration limits from move sign ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) AN532 AL+B0,WREG WREG,A+B0 AL+B1,WREG WREG,A+B1 AL+B2,WREG WREG,A+B2 ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) DS00532C-page 92 © 1997 Microchip Technology Inc. AN532 03E2 290A 03E3 326C 03E4 C3F7 03E5 03E6 03E7 03E8 03E9 03EA 03EB 03EC 03ED 13AB 13AC 13AD 13AE 290A 15AB 11AC 11AD 11AE 03EE 03EF 03F0 03F1 03F2 03F3 03F4 03F5 03F6 13A7 13A8 13A9 13AA 290A 15A7 11A8 11A9 11AA 03F7 03F7 2966 03F8 03F9 03FA 03FB 03FC 03FD 6A62 4A67 6A63 4A68 6A64 4A69 03FE 03FF 0400 0401 0402 1A69 1969 1968 1967 1966 M 01704 01705 01706 01707 M M M M M M M M M M M 01708 M M M M M M M M M M M 01709 minc 01710 01711 M M M M M M M M 01712 M M M M M M M CLRF CPFSGT GOTO NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC WREG, F MOVSIGN minc V V+B0, V+B1, V+B2, V+B3, WREG, V+B0, V+B1, V+B2, V+B3, A A+B0, A+B1, A+B2, A+B3, WREG, A+B0, A+B1, A+B2, A+B3, F F F F F F F F F F F F F F F F F F CLRF MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF RRC32 RLCF RRCF RRCF RRCF RRCF HMOVVAL+B0, F MOVVAL,HMOVVAL+B1 MOVVAL+B0,WREG WREG,HMOVVAL+B1+B0 MOVVAL+B1,WREG WREG,HMOVVAL+B1+B1 MOVVAL+B2,WREG WREG,HMOVVAL+B1+B2 HMOVVAL HMOVVAL+B3,W HMOVVAL+B3, F HMOVVAL+B2, F HMOVVAL+B1, F HMOVVAL+B0, F ; evaluate MOVVAL/2 ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; half move in Q8 ; move sign into carry bit © 1997 Microchip Technology Inc. DS00532C-page 93 0403 C43D 0404 0404 9F96 0405 C445 0406 2969 0407 0408 0409 040A 040B 040C 6A62 4A66 6A63 4A67 6A64 4A68 040D 9F64 040E 2B69 040F 0410 0411 0412 0413 0414 0415 0416 6AB3 0566 6AB4 0367 6AB5 0368 6AB6 0369 0417 6A69 0418 B580 0419 4A6C 041A 29AE 041B 041C 041D 041E 041F 0420 6A20 4AAB 6A21 4AAC 6A22 4AAD 0421 29AA 01713 01714 01715 vmode 01716 01717 01718 01719 01720 M M M M M M M M 01721 01722 01723 M M M M M M M M M M 01724 01725 01726 01727 01728 01729 01730 M M M M M M M M 01731 01732 M GOTO modeready BTFSC GOTO CLRF MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF BTFSC SETF SUB32 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB MOVFP SUBWFB MODETYPE,MSB tmode HMOVVAL+B3, F MOVVAL,HMOVVAL MOVVAL+B0,WREG WREG,HMOVVAL+B0 MOVVAL+B1,WREG WREG,HMOVVAL+B1 MOVVAL+B2,WREG WREG,HMOVVAL+B2 MOVVAL+B2,MSB HMOVVAL+B3, F MOVVBUF,HMOVVAL MOVVBUF+B0,WREG HMOVVAL+B0, F MOVVBUF+B1,WREG HMOVVAL+B1, F MOVVBUF+B2,WREG HMOVVAL+B2, F MOVVBUF+B3,WREG HMOVVAL+B3, F ; is it torque move? ; compute final minus initial velocity ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; ; ; ; ; ; ; ; get sub get sub get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of 4th byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) a into w b, save in b(B3) MOVFP ANDLW MOVPF CLRF MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CLRF MOV24 HMOVVAL+B3,WREG 0x80 WREG,MOVSIGN V+B3, F VL,V VL+B0,WREG WREG,V+B0 VL+B1,WREG WREG,V+B1 VL+B2,WREG WREG,V+B2 A+B3, F AL,A ; create appropriate velocity and ; acceleration limits from move sign ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) AN532 DS00532C-page 94 © 1997 Microchip Technology Inc. AN532 0422 0423 0424 0425 0426 0427 6A23 4AA7 6A24 4AA8 6A25 4AA9 0428 290A 0429 326C 042A C43D 042B 042C 042D 042E 042F 0430 0431 0432 0433 13AB 13AC 13AD 13AE 290A 15AB 11AC 11AD 11AE 0434 0435 0436 0437 0438 0439 043A 043B 043C 13A7 13A8 13A9 13AA 290A 15A7 11A8 11A9 11AA 043D 043D 2965 043E 9F64 043F 2B65 0440 2B95 0441 0441 6AC6 M M M M M M M 01733 01734 01735 01736 M M M M M M M M M M M 01737 M M M M M M M M M M M 01738 01739 01740 01741 01742 01743 01744 01745 01746 01747 01748 01749 01750 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CLRF CPFSGT GOTO NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC AL+B0,WREG WREG,A+B0 AL+B1,WREG WREG,A+B1 AL+B2,WREG WREG,A+B2 WREG, F MOVSIGN modeready V V+B0, V+B1, V+B2, V+B3, WREG, V+B0, V+B1, V+B2, V+B3, A A+B0, A+B1, A+B2, A+B3, WREG, A+B0, A+B1, A+B2, A+B3, F F F F F F F F F F F F F F F F F F ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) modeready CLRF BTFSC SETF SETF MOVVAL+B3, F MOVVAL+B2,MSB MOVVAL+B3, F SERVOFLAG, F ; set servoflag to restore servo ; if stopped ;************************************************************************* ; For PICMASTER Debug/servo tuning puporses only Purposes Only ; testCapCount MOVFP CAPCOUNT+B0,WREG © 1997 Microchip Technology Inc. DS00532C-page 95 0442 08C7 0443 4AC5 0444 0002 0445 0445 0446 0447 0448 0449 044A 044B 044C 044D 6A63 4A87 6A64 4A88 2995 E333 2999 8D98 C441 044E 0002 01751 01752 01753 01754 01755 01756 01757 01758 M M M M M M 01759 01760 01761 01762 01763 01764 01765 01766 01767 01768 01769 01770 01771 01772 01773 01774 01775 01776 01777 01778 01779 01780 01781 01782 01783 01784 01785 01786 01787 01788 01789 01790 01791 IORWF CAPCOUNT+B1,W MOVPF WREG,CAPFLAG ;************************************************************************* RETURN tmode MOV16 MOVFP MOVPF MOVFP MOVPF MOVVAL+B1,YPWM MOVVAL+B1+B0,WREG WREG,YPWM+B0 MOVVAL+B1+B1,WREG WREG,YPWM+B1 ; torque/voltage mode ; set new commanded value ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) CLRF SERVOFLAG, F CALL doTorque CLRF MOVFLAG, F BCF MOVSTAT,BIT5 goto testCapCount RETURN ; disable servo ; set pwm duty cycle ;***************************************************************************** ;***************************************************************************** ; NAME: doMove ; ; DESCRIPTION: In position mode, trapezoidal moves are performed. Phase1 ; and phase2 respectively, are the periods for the first and ; second halves of the move. The move time is defined as zero ; at the beginning of the move,T2 is the time at half the move, T1 is the time w ; begins,(the region of constant velocity reduces to a point ; in the case where maximum speed is not realized, and the ; trapezoidal move degenerates into a trianglular move, ; together with T1=T2), and TAU is the total time of the move. ; The accelerations are +-AL or 0. ; ; ; triangle speed trapezoidal speed ; ; ________________ ; / \ ; /\ / \ ; / \ / \ ; / \ / \ ; / \ / \ ; / \ / \ AN532 DS00532C-page 96 © 1997 Microchip Technology Inc. 044F 044F 290A 0450 156A 0451 116B AN532 01792 01793 01794 01795 01796 01797 01798 01799 01800 01801 01802 01803 01804 01805 01806 01807 01808 01809 01810 01811 01812 01813 01814 01815 01816 01817 01818 01819 01820 01821 01822 01823 01824 01825 01826 01827 01828 01829 01830 01831 01832 M M M M M 01833 ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; doMove INC16 0 T1=T2 TAU 0 T1 T2 TAU Let x denote the undershoot and y the overshoot commanded at adjacent sample times as half the move is crossed. In the case of a triangular move, the discretization error is given by error = min (2x,2y) For a trapezoidal move, the discretization error is error = min (2x,y-x) <= .5*(maximum commanded speed) This discretization error is resolved in the final sample time of the move by executing a step to the final position at zero speed. The method employed here the best possible performance with regard to discretization error without dynamically modifying velocity and acceleration limits. In velocity mode, ramp moves are performed. / final velocity / / / / initial velocity / 0 TAU MOVTIME WREG, F MOVTIME+B0, F MOVTIME+B1, F ; increment move time CLRF INCF ADDWFC © 1997 Microchip Technology Inc. DS00532C-page 97 0452 E5A8 0453 0454 0455 0455 0456 0457 0458 3396 C569 6ACB 3199 C51B 0458 0459 045A 045B 7CBB 7DBC 7EBD 7FBE 045C 045D 045E 045F 0460 0461 0462 0463 6A54 4ABB 6A55 4ABC 6A56 4ABD 6A57 4ABE 0464 0465 0466 0467 0468 0469 046A 046B 6A66 0FBB 6A67 11BC 6A68 11BD 6A69 11BE 046C 046D 046E 046F 0470 0471 0472 6AAF 05BB 6AB0 03BC 6AB1 03BD 6AB2 01834 01835 01836 01837 01838 pmove 01839 01840 01841 01842 phase1 01843 M M M M M M 01844 M M M M M M M M M M 01845 M M M M M M M M M M 01846 M M M M M M M M CALL TSTFSZ GOTO MOVFP CPFSEQ GOTO MVFP32 MOVFP MOVFP MOVFP MOVFP MOV32 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC SUB32 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB MOVFP doPosVel MODETYPE vmove ONE,WREG MOVFLAG phase2 MOVDEL,MOVTMP MOVDEL+B0,MOVTMP+B0 MOVDEL+B1,MOVTMP+B1 MOVDEL+B2,MOVTMP+B2 MOVDEL+B3,MOVTMP+B3 OPOSITION,MOVDEL OPOSITION+B0,WREG WREG,MOVDEL+B0 OPOSITION+B1,WREG WREG,MOVDEL+B1 OPOSITION+B2,WREG WREG,MOVDEL+B2 OPOSITION+B3,WREG WREG,MOVDEL+B3 HMOVVAL,MOVDEL HMOVVAL+B0,WREG MOVDEL+B0, F HMOVVAL+B1,WREG MOVDEL+B1, F HMOVVAL+B2,WREG MOVDEL+B2, F HMOVVAL+B3,WREG MOVDEL+B3, F MOVPBUF,MOVDEL ; evaluate iterative equations ; test if in phase1 ; save previous discretization error ; ; ; ; move move move move A(B0) A(B1) A(B2) A(B3) to to to to B(B0) B(B1) B(B2) B(B3) ; test if half move ; ; ; ; ; ; ; ; get byte of a move to b(B0) get byte of a move to b(B1) get byte of a move to b(B2) get byte of a move to b(B3) into w into w into w into w ; ; ; ; ; ; ; ; get add get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of 4th byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) a into w b, save in b(B3) AN532 MOVPBUF+B0,WREG MOVDEL+B0, F MOVPBUF+B1,WREG MOVDEL+B1, F MOVPBUF+B2,WREG MOVDEL+B2, F MOVPBUF+B3,WREG ; ; ; ; ; ; ; get sub get sub get sub get lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) a into w DS00532C-page 98 © 1997 Microchip Technology Inc. AN532 0473 03BE 0474 976C 0475 C47F 0476 0477 0478 0479 047A 047B 047C 047D 047E 13BB 13BC 13BD 13BE 290A 15BB 11BC 11BD 11BE 047F 047F 97BE 0480 C4E5 0481 6A6D 0482 086E 0483 330A 0484 C4B8 0485 0486 0487 0488 0489 048A 048B 048C 048D 13A7 13A8 13A9 13AA 290A 15A7 11A8 11A9 11AA 048E 048F 0490 0491 6ABB 0F1C 6ABC 111D M M 01847 01848 01849 M M M M M M M M M M M 01850 mpos1 01851 01852 01853 01854 M M M M 01855 01856 01857 01858 01859 M M M M M M M M M M M 01860 M M M M M SUBWFB BTFSS GOTO NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC MOVDEL+B3, F MOVSIGN,MSB mpos1 MOVDEL MOVDEL+B0, MOVDEL+B1, MOVDEL+B2, MOVDEL+B3, WREG, F MOVDEL+B0, MOVDEL+B1, MOVDEL+B2, MOVDEL+B3, F F F F F F F F ; sub 4th byte of b, save in b(B3) BTFSS GOTO TFSZ16 MOVFP IORWF TSTFSZ MOVDEL+B3,MSB speedup T1 T1+B0,WREG T1+B1,W WREG ; continue to speed up if in phase1 ; if T1=0, maximum velocity not ; reached, so T1=T2, otherwise T1 ; has been set in speedup GOTO NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC ADD32 MOVFP ADDWF MOVFP ADDWFC t2net1 A A+B0, A+B1, A+B2, A+B3, WREG, A+B0, A+B1, A+B2, A+B3, F F F F F F F F F ; test x-y < 0 ; ; ; ; get add get add lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) ; negate A for speeddown MOVDEL,MOVTMP MOVDEL+B0,WREG MOVTMP+B0, F MOVDEL+B1,WREG MOVTMP+B1, F © 1997 Microchip Technology Inc. DS00532C-page 99 0492 0493 0494 0495 6ABD 111E 6ABE 111F 0496 971F 0497 C4AE 0498 2B6F 0499 2B70 049A 049B 049C 049D 049E 049F 04A0 04A1 04A2 13A7 13A8 13A9 13AA 290A 15A7 11A8 11A9 11AA 04A3 E5CA 04A4 04A5 04A6 04A7 04A8 04A9 04AA 04AB 04AC 13A7 13A8 13A9 13AA 290A 15A7 11A8 11A9 11AA 04AD E5A8 04AE 04AE 04AF 04B0 04B1 6A6A 0F6F 6A6B 1170 04B2 6A6F M M M M M 01861 01862 01863 01864 01865 01866 M M M M M M M M M M M 01867 01868 M M M M M M M M M M M 01869 01870 triok 01871 M M M M M M 01872 M M MOVFP ADDWFC MOVFP ADDWFC BTFSS GOTO SETF SETF NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC CALL NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC CALL ADD16 MOVFP ADDWF MOVFP ADDWFC MOV16 MOVFP MOVDEL+B2,WREG MOVTMP+B2, F MOVDEL+B3,WREG MOVTMP+B3, F MOVTMP+B3,MSB triok T2+B0, F T2+B1, F A A+B0, A+B1, A+B2, A+B3, WREG, A+B0, A+B1, A+B2, A+B3, F F F F F F F F F ; ; ; ; get add get add 3rd 3rd 4th 4th byte byte byte byte of of of of a into w b, save in b(B2) a into w b, save in b(B3) ; if new discretization error larger, ; backup to define T2, otherwise ok ; set T2=-1 for backup ; negate A to undo undoPosVel A A+B0, A+B1, A+B2, A+B3, WREG, A+B0, A+B1, A+B2, A+B3, F F F F F F F F F ; negate A again for speeddown doPosVel MOVTIME,T2 MOVTIME+B0,WREG T2+B0, F MOVTIME+B1,WREG T2+B1, F T2,T1 T2+B0,WREG ; and reevaluate iterative equations ; add time to T2 ; ; ; ; get add get add lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) AN532 ; get byte of a into w DS00532C-page 100 © 1997 Microchip Technology Inc. AN532 04B3 4A6D 04B4 6A70 04B5 4A6E 04B6 1599 04B7 C50E 04B8 04B8 2B6F 04B9 2B70 04BA 04BB 04BC 04BD 6A6A 0F6F 6A6B 1170 04BE 04BF 04C0 04C1 781C 791D 7A1E 7B1F 04C2 04C3 04C4 04C5 04C6 8804 1B1C 1B1D 1B1E 1B1F 04C7 04C8 04C9 04CA 04CB 04CC 04CD 04CE 6A18 0F1C 6A19 111D 6A1A 111E 6A1B 111F 04CF 6ABB M M M M 01873 01874 01875 01876 t2net1 01877 01878 01879 M M M M M M 01880 01881 M M M M M M 01882 M M M M M M M 01883 M M M M M M M M M M 01884 M M MOVPF MOVFP MOVPF INCF GOTO WREG,T1+B0 T2+B1,WREG WREG,T1+B1 MOVFLAG, F mvok ; move to b(B0) ; get byte of a into w ; move to b(B1) ; increment move flag for phase2 ; execute last phase1 move SETF SETF ADD16 MOVFP ADDWF MOVFP ADDWFC T2+B0, F T2+B1, F MOVTIME,T2 MOVTIME+B0,WREG T2+B0, F MOVTIME+B1,WREG T2+B1, F ; set T2=-1 for backup ; add time to T2 ; ; ; ; get add get add lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) MVFP32 MOVFP MOVFP MOVFP MOVFP RLC32 BCF RLCF RLCF RLCF RLCF ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC ADD32 MOVFP MOVTMP,TMP MOVTMP+B0,TMP+B0 MOVTMP+B1,TMP+B1 MOVTMP+B2,TMP+B2 MOVTMP+B3,TMP+B3 MOVTMP _carry MOVTMP+B0, MOVTMP+B1, MOVTMP+B2, MOVTMP+B3, TMP,MOVTMP TMP+B0,WREG MOVTMP+B0, F TMP+B1,WREG MOVTMP+B1, F TMP+B2,WREG MOVTMP+B2, F TMP+B3,WREG MOVTMP+B3, F MOVDEL,MOVTMP MOVDEL+B0,WREG ; test if 3x-y < 0 ; ; ; ; move move move move A(B0) A(B1) A(B2) A(B3) to to to to B(B0) B(B1) B(B2) B(B3) F F F F ; ; ; ; ; ; ; ; get add get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of 4th byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) a into w b, save in b(B3) ; get lowest byte of a into w © 1997 Microchip Technology Inc. DS00532C-page 101 04D0 04D1 04D2 04D3 04D4 04D5 04D6 04D7 04D8 04D9 04DA 04DB 0F1C 6ABC 111D 6ABD 111E 6ABE 111F 971F C4DB 2BBF 2BC0 04DB 04DC 04DD 04DE 6A6F 0FBF 6A70 11C0 04DF 04E0 04E1 04E2 6A6D 05BF 6A6E 03C0 04E3 1599 04E4 C50E 04E5 04E5 04E6 04E7 04E8 7CAB 7DAC 7EAD 7FAE 04E9 04EA 04EB 04EC 04ED 04EE 04EF 6AB3 051C 6AB4 031D 6AB5 031E 6AB6 M M M M M M M M 01885 01886 01887 01888 01889 trapok 01890 M M M M M M 01891 M M M M M M 01892 01893 01894 01895 speedup 01896 M M M M M M 01897 M M M M M M M M ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC BTFSS GOTO SETF SETF ADD16 MOVFP ADDWF MOVFP ADDWFC SUB16 MOVFP SUBWF MOVFP SUBWFB INCF GOTO MOVTMP+B0, F MOVDEL+B1,WREG MOVTMP+B1, F MOVDEL+B2,WREG MOVTMP+B2, F MOVDEL+B3,WREG MOVTMP+B3, F MOVTMP+B3,MSB trapok PH2FLAT+B0, F PH2FLAT+B1, F T2,PH2FLAT T2+B0,WREG PH2FLAT+B0, F T2+B1,WREG PH2FLAT+B1, F T1,PH2FLAT T1+B0,WREG PH2FLAT+B0, F T1+B1,WREG PH2FLAT+B1, F MOVFLAG, F mvok ; ; ; ; ; ; ; add get add get add get add lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of 4th byte of of b, save a into w b, save in a into w b, save in a into w b, save in in b(B0) b(B1) b(B2) b(B3) ; if new discretization error larger, ; take one more flat step ; ; ; ; get add get add lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) ; ; ; ; get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) ; increment move flag for phase2 ; execute last phase1 move MVFP32 MOVFP MOVFP MOVFP MOVFP SUB32 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB MOVFP V,MOVTMP V+B0,MOVTMP+B0 V+B1,MOVTMP+B1 V+B2,MOVTMP+B2 V+B3,MOVTMP+B3 MOVVBUF,MOVTMP ; test if maximum velocity reached ; ; ; ; move move move move A(B0) A(B1) A(B2) A(B3) to to to to B(B0) B(B1) B(B2) B(B3) AN532 MOVVBUF+B0,WREG MOVTMP+B0, F MOVVBUF+B1,WREG MOVTMP+B1, F MOVVBUF+B2,WREG MOVTMP+B2, F MOVVBUF+B3,WREG ; ; ; ; ; ; ; get sub get sub get sub get lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) a into w DS00532C-page 102 © 1997 Microchip Technology Inc. AN532 04F0 031F 04F1 976C 04F2 C4FC 04F3 04F4 04F5 04F6 04F7 04F8 04F9 04FA 04FB 131C 131D 131E 131F 290A 151C 111D 111E 111F 04FC 04FC 971F 04FD C50E 04FE 6A6D 04FF 086E 0500 330A 0501 C50E 0502 E5CA 0503 0504 0505 0506 29A7 29A8 29A9 29AA 0507 E5A8 0508 2B6D 0509 2B6E 050A 050B 050C 050D 050E 6A6A 0F6D 6A6B 116E M M 01898 01899 01900 M M M M M M M M M M M 01901 mpos 01902 01903 01904 01905 M M M M 01906 01907 01908 01909 M M M M M 01910 01911 01912 01913 M M M M M M 01914 mvok 01915 M SUBWFB BTFSS GOTO NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC MOVTMP+B3, F MOVSIGN,MSB mpos MOVTMP MOVTMP+B0, MOVTMP+B1, MOVTMP+B2, MOVTMP+B3, WREG, F MOVTMP+B0, MOVTMP+B1, MOVTMP+B2, MOVTMP+B3, F F F F F F F F ; sub 4th byte of b, save in b(B3) BTFSS GOTO TFSZ16 MOVFP IORWF TSTFSZ GOTO CALL CLR32 CLRF CLRF CLRF CLRF CALL SETF SETF ADD16 MOVFP ADDWF MOVFP ADDWFC MOVTMP+B3,MSB mvok T1 T1+B0,WREG T1+B1,W WREG ; if not, execute move ; if so, check to see if T1 has ; already been set mvok undoPosVel A A+B0, F A+B1, F A+B2, F A+B3, F doPosVel T1+B0, F T1+B1, F MOVTIME,T1 MOVTIME+B0,WREG T1+B0, F MOVTIME+B1,WREG T1+B1, F ; if not, backup and redo iterative ; equations, resulting in an actual ; maximum speed <= VL ; evaluate T1 ; ; ; ; get add get add lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) MOV24 MOVPBUF+B1,POSITION ; move Q8 calculated position to Q0 commanded position © 1997 Microchip Technology Inc. DS00532C-page 103 050E 050F 0510 0511 0512 0513 6AB0 4A58 6AB1 4A59 6AB2 4A5A 0514 0515 0516 0517 0518 0519 6AB3 4A5B 6AB4 4A5C 6AB5 4A5D 051A 0002 051B 051B 051C 051D 051E 6ABF 08C0 330A C53F 051F 0520 0521 0522 0523 0524 6AB3 08B4 08B5 08B6 330A C55C 0525 2999 0526 8E98 0527 8D98 0528 0529 052A 052B 29A7 29A8 29A9 29AA M M M M M M M 01916 M M M M M M M M 01917 01918 01919 01920 phase2 01921 M M M M 01922 01923 01924 M M M M M M 01925 01926 01927 01928 01929 01930 M M M M M 01931 M MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF RETURN MOVPBUF+B1+B0,WREG WREG,POSITION+B0 MOVPBUF+B1+B1,WREG WREG,POSITION+B1 MOVPBUF+B1+B2,WREG WREG,POSITION+B2 MOVVBUF+B0,VELOCITY MOVVBUF+B0+B0,WREG WREG,VELOCITY+B0 MOVVBUF+B0+B1,WREG WREG,VELOCITY+B1 MOVVBUF+B0+B2,WREG WREG,VELOCITY+B2 ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; move Q0 calculated velocity to Q0 commanded velocity ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) TFSZ16 MOVFP IORWF TSTFSZ GOTO TFSZ32 MOVFP IORWF IORWF IORWF TSTFSZ GOTO CLRF BCF BCF CLR32 CLRF CLRF CLRF CLRF MOV16 PH2FLAT PH2FLAT+B0,WREG PH2FLAT+B1,W WREG flat MOVVBUF MOVVBUF+B0,WREG MOVVBUF+B1,W MOVVBUF+B2,W MOVVBUF+B3,W WREG mready MOVFLAG, F MOVSTAT,BIT6 MOVSTAT,BIT5 A A+B0, F A+B1, F A+B2, F A+B3, F MOVTIME,TAU ; is flat section finished? ; is velocity zero? ; if not, execute move ; ; ; ; if so, clear MOVFLAG clear motion status flag clear move in progress flag set zero velocity and acceleration, AN532 DS00532C-page 104 © 1997 Microchip Technology Inc. AN532 052C 052D 052E 052F 6A6A 4A71 6A6B 4A72 0530 0531 0532 0533 0534 0535 0536 0537 6A54 4AAF 6A55 4AB0 6A56 4AB1 6A57 4AB2 0538 0539 053A 053B 053C 053D 6A62 0FB0 6A63 11B1 6A64 11B2 053E C55C 053F 053F 2B1C 0540 2B1D 0541 0542 0543 0544 6A1C 0FBF 6A1D 11C0 0545 0546 0547 0548 6ABF 08C0 330A C55C 0549 29AA M M M M M 01932 M M M M M M M M M M 01933 M M M M M M M M 01934 01935 01936 flat 01937 01938 01939 M M M M M M 01940 01941 M M M M 01942 01943 01944 01945 MOVFP MOVPF MOVFP MOVPF MOV32 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF ADD24 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC GOTO MOVTIME+B0,WREG WREG,TAU+B0 MOVTIME+B1,WREG WREG,TAU+B1 OPOSITION,MOVPBUF OPOSITION+B0,WREG WREG,MOVPBUF+B0 OPOSITION+B1,WREG WREG,MOVPBUF+B1 OPOSITION+B2,WREG WREG,MOVPBUF+B2 OPOSITION+B3,WREG WREG,MOVPBUF+B3 MOVVAL,MOVPBUF+B1 MOVVAL+B0,WREG MOVPBUF+B1+B0, F MOVVAL+B1,WREG MOVPBUF+B1+B1, F MOVVAL+B2,WREG MOVPBUF+B1+B2, F mready ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) ; execute last move to P(0)+MOVVAL ; ; ; ; ; ; ; ; get byte of a move to b(B0) get byte of a move to b(B1) get byte of a move to b(B2) get byte of a move to b(B3) into w into w into w into w ; ; ; ; ; ; get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) SETF SETF ADD16 MOVFP ADDWF MOVFP ADDWFC MOVTMP+B0, F MOVTMP+B1, F MOVTMP,PH2FLAT MOVTMP+B0,WREG PH2FLAT+B0, F MOVTMP+B1,WREG PH2FLAT+B1, F ; decrement by one use DEC16 ; ; ; ; get add get add lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) TFSZ16 MOVFP IORWF TSTFSZ GOTO CLRF MOV24 PH2FLAT PH2FLAT+B0,WREG PH2FLAT+B1,W WREG mready A+B3, F AL,A ; begin speed down section © 1997 Microchip Technology Inc. DS00532C-page 105 054A 054B 054C 054D 054E 054F 6A23 4AA7 6A24 4AA8 6A25 4AA9 0550 290A 0551 316C 0552 C55C 0553 0554 0555 0556 0557 0558 0559 055A 055B 13A7 13A8 13A9 13AA 290A 15A7 11A8 11A9 11AA 055C 055C 055D 055E 055F 0560 0561 6AB0 4A58 6AB1 4A59 6AB2 4A5A 0562 0563 0564 0565 0566 0567 6AB3 4A5B 6AB4 4A5C 6AB5 4A5D 0568 0002 0569 M M M M M M M M 01946 01947 01948 01949 M M M M M M M M M M M 01950 01951 mready 01952 M M M M M M M M 01953 M M M M M M M M 01954 01955 01956 vmove 01957 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CLRF CPFSEQ GOTO NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC AL+B0,WREG WREG,A+B0 AL+B1,WREG WREG,A+B1 AL+B2,WREG WREG,A+B2 WREG, F MOVSIGN mready A A+B0, A+B1, A+B2, A+B3, WREG, A+B0, A+B1, A+B2, A+B3, F F F F F F F F F ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF RETURN MOVPBUF+B1,POSITION MOVPBUF+B1+B0,WREG WREG,POSITION+B0 MOVPBUF+B1+B1,WREG WREG,POSITION+B1 MOVPBUF+B1+B2,WREG WREG,POSITION+B2 MOVVBUF+B0,VELOCITY MOVVBUF+B0+B0,WREG WREG,VELOCITY+B0 MOVVBUF+B0+B1,WREG WREG,VELOCITY+B1 MOVVBUF+B0+B2,WREG WREG,VELOCITY+B2 ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) AN532 MVFP32 MOVVAL,MOVTMP ; test if final velocity reached DS00532C-page 106 © 1997 Microchip Technology Inc. AN532 0569 056A 056B 056C 7C62 7D63 7E64 7F65 056D 056E 056F 0570 0571 0572 0573 0574 6AB3 051C 6AB4 031D 6AB5 031E 6AB6 031F 0575 976C 0576 C580 0577 0578 0579 057A 057B 057C 057D 057E 057F 131C 131D 131E 131F 290A 151C 111D 111E 111F 0580 0580 971F 0581 C59B 0582 0583 0584 0585 29A7 29A8 29A9 29AA 0586 0587 0588 0589 6A62 4AB3 6A63 4AB4 M M M M M M 01958 M M M M M M M M M M 01959 01960 01961 M M M M M M M M M M M 01962 vmpos 01963 01964 01965 01966 M M M M M 01967 M M M M M MOVFP MOVFP MOVFP MOVFP SUB32 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB MOVFP SUBWFB BTFSS GOTO NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC MOVVAL+B0,MOVTMP+B0 MOVVAL+B1,MOVTMP+B1 MOVVAL+B2,MOVTMP+B2 MOVVAL+B3,MOVTMP+B3 MOVVBUF,MOVTMP MOVVBUF+B0,WREG MOVTMP+B0, F MOVVBUF+B1,WREG MOVTMP+B1, F MOVVBUF+B2,WREG MOVTMP+B2, F MOVVBUF+B3,WREG MOVTMP+B3, F MOVSIGN,MSB vmpos MOVTMP MOVTMP+B0, MOVTMP+B1, MOVTMP+B2, MOVTMP+B3, WREG, F MOVTMP+B0, MOVTMP+B1, MOVTMP+B2, MOVTMP+B3, F F F F F F F F ; ; ; ; move move move move A(B0) A(B1) A(B2) A(B3) to to to to B(B0) B(B1) B(B2) B(B3) ; ; ; ; ; ; ; ; get sub get sub get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of 4th byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) a into w b, save in b(B3) BTFSS GOTO CLR32 CLRF CLRF CLRF CLRF MOV32 MOVFP MOVPF MOVFP MOVPF MOVTMP+B3,MSB vmoveok A A+B0, A+B1, A+B2, A+B3, ; if not, continue ; if so, set A=0 and continue with F F F F ; move unless the final velocity ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) MOVVAL,MOVVBUF MOVVAL+B0,WREG WREG,MOVVBUF+B0 MOVVAL+B1,WREG WREG,MOVVBUF+B1 © 1997 Microchip Technology Inc. DS00532C-page 107 058A 058B 058C 058D 6A64 4AB5 6A65 4AB6 058E 2999 058F 8D98 0590 0591 0592 0593 6A6A 4A71 6A6B 4A72 0594 0595 0596 0597 0598 0599 6A62 0863 0864 0865 330A C59B 059A 8E98 059B 059B 059C 059D 059E 059F 05A0 6AB0 4A58 6AB1 4A59 6AB2 4A5A 05A1 05A2 05A3 05A4 05A5 05A6 6AB3 4A5B 6AB4 4A5C 6AB5 4A5D 05A7 0002 M MOVFP M MOVPF M MOVFP M MOVPF M 01968 01969 CLRF 01970 BCF 01971 MOV16 M M MOVFP M MOVPF M MOVFP M MOVPF M 01972 TFSZ32 M M MOVFP M IORWF M IORWF M IORWF M TSTFSZ 01973 GOTO 01974 01975 BCF 01976 01977 vmoveok 01978 MOV24 M M MOVFP M MOVPF M MOVFP M MOVPF M MOVFP M MOVPF M 01979 MOV24 M M MOVFP M MOVPF M MOVFP M MOVPF M MOVFP M MOVPF M 01980 RETURN 01981 MOVVAL+B2,WREG WREG,MOVVBUF+B2 MOVVAL+B3,WREG WREG,MOVVBUF+B3 ; ; ; ; get byte of a into w move to b(B2) get byte of a into w move to b(B3) MOVFLAG, F MOVSTAT,BIT5 MOVTIME,TAU MOVTIME+B0,WREG WREG,TAU+B0 MOVTIME+B1,WREG WREG,TAU+B1 MOVVAL MOVVAL+B0,WREG MOVVAL+B1,W MOVVAL+B2,W MOVVAL+B3,W WREG vmoveok MOVSTAT,BIT6 ; is zero. ; clear MOVFLAG ; clear move in progress flag ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) ; if final velocity is zero, clear ; motion status flag MOVPBUF+B1,POSITION MOVPBUF+B1+B0,WREG WREG,POSITION+B0 MOVPBUF+B1+B1,WREG WREG,POSITION+B1 MOVPBUF+B1+B2,WREG WREG,POSITION+B2 MOVVBUF+B0,VELOCITY MOVVBUF+B0+B0,WREG WREG,VELOCITY+B0 MOVVBUF+B0+B1,WREG WREG,VELOCITY+B1 MOVVBUF+B0+B2,WREG WREG,VELOCITY+B2 ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) AN532 DS00532C-page 108 05A8 05A8 05A9 05AA 05AB 05AC 05AD 05AE 05AF 6AB3 0FAF 6AB4 11B0 6AB5 11B1 6AB6 11B2 05B0 05B1 05B2 05B3 05B4 05B5 05B6 05B7 6AA7 0FB3 6AA8 11B4 6AA9 11B5 6AAA 11B6 © 1997 Microchip Technology Inc. 05B8 05B9 05BA 05BB 7CA7 7DA8 7EA9 7FAA AN532 01982 01983 01984 01985 01986 01987 01988 01989 01990 01991 01992 01993 01994 01995 01996 01997 01998 M M M M M M M M M M 01999 M M M M M M M M M M 02000 02001 M M M M M M 02002 ;***************************************************************************** ;***************************************************************************** ; NAME: doPosVel ; ; DESCRIPTION: Evaluates the iterative equations for trapezoidal move ; generation ; ; V(k)=V(k-1)+A, P(k)=P(k-1)+V(k-1)+A/2, ; ; where abs(A)={AL,0} depending on the region of the trapezoid ; being executed. ; doPosVel ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC MOVVBUF,MOVPBUF MOVVBUF+B0,WREG MOVPBUF+B0, F MOVVBUF+B1,WREG MOVPBUF+B1, F MOVVBUF+B2,WREG MOVPBUF+B2, F MOVVBUF+B3,WREG MOVPBUF+B3, F A,MOVVBUF A+B0,WREG MOVVBUF+B0, A+B1,WREG MOVVBUF+B1, A+B2,WREG MOVVBUF+B2, A+B3,WREG MOVVBUF+B3, ; P(k-1)+V(k-1) ; ; ; ; ; ; ; ; get add get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of 4th byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) a into w b, save in b(B3) ; V(k)=V(k-1)+A ; ; ; ; ; ; ; ; get add get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of 4th byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) a into w b, save in b(B3) F F F F MVFP32 MOVFP MOVFP MOVFP MOVFP RRC32 A,MOVTMP A+B0,MOVTMP+B0 A+B1,MOVTMP+B1 A+B2,MOVTMP+B2 A+B3,MOVTMP+B3 MOVTMP ; compute A/2 ; ; ; ; move move move move A(B0) A(B1) A(B2) A(B3) to to to to B(B0) B(B1) B(B2) B(B3) © 1997 Microchip Technology Inc. DS00532C-page 109 05BC 05BD 05BE 05BF 05C0 1A1F 191F 191E 191D 191C 05C1 05C2 05C3 05C4 05C5 05C6 05C7 05C8 6A1C 0FAF 6A1D 11B0 6A1E 11B1 6A1F 11B2 05C9 0002 05CA 05CA 05CB 05CC 05CD 05CE 05CF 6AA7 05B3 6AA8 03B4 6AA9 03B5 M M M M M M M 02003 02004 M M M M M M M M M M 02005 02006 02007 02008 02009 02010 02011 02012 02013 02014 02015 02016 02017 02018 02019 02020 02021 02022 02023 02024 02025 M M M M M M M RLCF RRCF RRCF RRCF RRCF MOVTMP+B3,W MOVTMP+B3, F MOVTMP+B2, F MOVTMP+B1, F MOVTMP+B0, F ; move sign into carry bit ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC MOVTMP,MOVPBUF MOVTMP+B0,WREG MOVPBUF+B0, F MOVTMP+B1,WREG MOVPBUF+B1, F MOVTMP+B2,WREG MOVPBUF+B2, F MOVTMP+B3,WREG MOVPBUF+B3, F ; P(k)=P(k-1)+V(k-1)+A/2, ; ; ; ; ; ; ; ; get add get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of 4th byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) a into w b, save in b(B3) RETURN ;***************************************************************************** ;***************************************************************************** ; NAME: undoPosVel ; ; DESCRIPTION: Backward iteration of the equations for trapezoidal move ; generation ; ; V(k-1)=V(k)-A, P(k-1)=P(k)-V(k-1)-A/2, ; ; where abs(A)={AL,0} depending on the region of the trapezoid ; being executed. This routine is used to reverse a step about ; to be made beyond a decision point. ; undoPosVel SUB32 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB A,MOVVBUF A+B0,WREG MOVVBUF+B0, F A+B1,WREG MOVVBUF+B1, F A+B2,WREG MOVVBUF+B2, F ; V(k-1)=V(k)-A AN532 ; ; ; ; ; ; get sub get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) DS00532C-page 110 © 1997 Microchip Technology Inc. AN532 05D0 6AAA 05D1 03B6 05D2 05D3 05D4 05D5 05D6 05D7 05D8 05D9 6AB3 05AF 6AB4 03B0 6AB5 03B1 6AB6 03B2 05DA 05DB 05DC 05DD 7CA7 7DA8 7EA9 7FAA 05DE 05DF 05E0 05E1 05E2 1A1F 191F 191E 191D 191C 05E3 05E4 05E5 05E6 05E7 05E8 05E9 05EA 6A1C 05AF 6A1D 03B0 6A1E 03B1 6A1F 03B2 05EB 0002 M MOVFP A+B3,WREG ; get 4th byte of a into w M SUBWFB MOVVBUF+B3, F ; sub 4th byte of b, save in b(B3) M 02026 SUB32 MOVVBUF,MOVPBUF ; P(k)-V(k-1) M M MOVFP MOVVBUF+B0,WREG ; get lowest byte of a into w M SUBWF MOVPBUF+B0, F ; sub lowest byte of b, save in b(B0) M MOVFP MOVVBUF+B1,WREG ; get 2nd byte of a into w M SUBWFB MOVPBUF+B1, F ; sub 2nd byte of b, save in b(B1) M MOVFP MOVVBUF+B2,WREG ; get 3rd byte of a into w M SUBWFB MOVPBUF+B2, F ; sub 3rd byte of b, save in b(B2) M MOVFP MOVVBUF+B3,WREG ; get 4th byte of a into w M SUBWFB MOVPBUF+B3, F ; sub 4th byte of b, save in b(B3) M 02027 02028 MVFP32 A,MOVTMP ; compute A/2 M M MOVFP A+B0,MOVTMP+B0 ; move A(B0) to B(B0) M MOVFP A+B1,MOVTMP+B1 ; move A(B1) to B(B1) M MOVFP A+B2,MOVTMP+B2 ; move A(B2) to B(B2) M MOVFP A+B3,MOVTMP+B3 ; move A(B3) to B(B3) M 02029 RRC32 MOVTMP M M RLCF MOVTMP+B3,W ; move sign into carry bit M RRCF MOVTMP+B3, F M RRCF MOVTMP+B2, F M RRCF MOVTMP+B1, F M RRCF MOVTMP+B0, F M 02030 02031 SUB32 MOVTMP,MOVPBUF ; P(k-1)=P(k)-V(k-1)-A/2, M M MOVFP MOVTMP+B0,WREG ; get lowest byte of a into w M SUBWF MOVPBUF+B0, F ; sub lowest byte of b, save in b(B0) M MOVFP MOVTMP+B1,WREG ; get 2nd byte of a into w M SUBWFB MOVPBUF+B1, F ; sub 2nd byte of b, save in b(B1) M MOVFP MOVTMP+B2,WREG ; get 3rd byte of a into w M SUBWFB MOVPBUF+B2, F ; sub 3rd byte of b, save in b(B2) M MOVFP MOVTMP+B3,WREG ; get 4th byte of a into w M SUBWFB MOVPBUF+B3, F ; sub 4th byte of b, save in b(B3) M 02032 02033 RETURN 02034 02035 ;***************************************************************************** 02036 © 1997 Microchip Technology Inc. DS00532C-page 111 05EC 05EC 78B7 05ED 79B8 05EE 05EE 05EF 05F0 05F1 05F2 4C0A 4BB7 310C C5EE 4AB8 05F3 2978 05F4 05F5 05F6 05F7 6AB7 4A79 6AB8 4A7A 05F8 05F9 05FA 05FB 6A18 0579 6A19 037A 05FC 78B9 05FD 79BA 05FE 02037 02038 02039 02040 02041 02042 02043 02044 02045 02046 02047 M M M M 02048 02049 02050 02051 02052 02053 02054 02055 02056 02057 M M M M M M 02058 M M M M M M 02059 02060 02061 02062 M M M M 02063 ;***************************************************************************** ; NAME: doMPosMVel ; ; DESCRIPTION: Calculates current position from UpCount and DownCount ; doMPosMVel ; Do UpCounter first MVFP16 MOVFP MOVFP readUp MOVPF MOVPF CPFSEQ GOTO MOVPF CLRF MOV16 MOVFP MOVPF MOVFP MOVPF SUB16 MOVFP SUBWF MOVFP SUBWFB TMR0H,WREG TMR0L,UPCOUNT+B0 TMR0H readUp WREG,UPCOUNT+B1 MVELOCITY+B0, F UPCOUNT,MVELOCITY+B1 UPCOUNT+B0,WREG WREG,MVELOCITY+B1+B0 UPCOUNT+B1,WREG WREG,MVELOCITY+B1+B1 TMP+B0,MVELOCITY+B1 TMP+B0+B0,WREG MVELOCITY+B1+B0, F TMP+B0+B1,WREG MVELOCITY+B1+B1, F ; ; ; ; get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) UPCOUNT,TMP+B0 UPCOUNT+B0,TMP+B0+B0 UPCOUNT+B1,TMP+B0+B1 ; save old upcount ; move A(B0) to B(B0) ; move A(B1) to B(B1) ; Skip next if HI hasn’t changed ; HI changed, re-read LO ; OK to store HI now ; clear bits below binary point ; compute upcount increment ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) ; Now do DownCounter AN532 MVFP16 MOVFP MOVFP readDown DOWNCOUNT,TMP+B0 DOWNCOUNT+B0,TMP+B0+B0 DOWNCOUNT+B1,TMP+B0+B1 ; save old downcount ; move A(B0) to B(B0) ; move A(B1) to B(B1) DS00532C-page 112 © 1997 Microchip Technology Inc. AN532 05FE 05FF 0600 0601 0602 0603 B802 530A 52B9 3113 C5FE 4ABA 0604 7AB9 0605 7BBA 0606 0607 0608 0609 6A18 051A 6A19 031B 060A 060B 060C 060D 6A1A 0579 6A1B 037A 060E 297B 060F 9F7A 0610 2B7B 0611 0612 0613 0614 0615 0616 6A79 0F75 6A7A 1176 6A7B 1177 0617 0002 02064 02065 02066 02067 02068 02069 02070 02071 M M M M 02072 M M M M M M 02073 02074 M M M M M M 02075 02076 02077 02078 02079 02080 02081 02082 M M M M M M M M 02083 02084 02085 02086 MOVLB MOVPF MOVPF CPFSEQ GOTO MOVPF MVFP16 MOVFP MOVFP SUB16 MOVFP SUBWF MOVFP SUBWFB BANK2 TMR3H,WREG TMR3L,DOWNCOUNT+B0 TMR3H readDown WREG,DOWNCOUNT+B1 DOWNCOUNT+B0,TMP+B2 ; timers in Bank 2 ; Skip next if HI hasn’t changed ; HI changed, re-read LO ; OK to store HI now ; compute downcount increment DOWNCOUNT+B0+B0,TMP+B2+B0 ; move A(B0) to B(B0) DOWNCOUNT+B0+B1,TMP+B2+B1 ; move A(B1) to B(B1) TMP+B0,TMP+B2 TMP+B0+B0,WREG TMP+B2+B0, F TMP+B0+B1,WREG TMP+B2+B1, F ; ; ; ; get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) SUB16 MOVFP SUBWF MOVFP SUBWFB TMP+B2,MVELOCITY+B1 TMP+B2+B0,WREG MVELOCITY+B1+B0, F TMP+B2+B1,WREG MVELOCITY+B1+B1, F ; compute new measured velocity ; ; ; ; get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) CLRF BTFSC SETF MVELOCITY+B3, F MVELOCITY+B2,MSB MVELOCITY+B3, F ; sign extend measured velocity for ; 24 bit addition to measured position ADD24 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MVELOCITY+B1,MPOSITION MVELOCITY+B1+B0,WREG MPOSITION+B0, F MVELOCITY+B1+B1,WREG MPOSITION+B1, F MVELOCITY+B1+B2,WREG MPOSITION+B2, F ; compute new measured position ; ; ; ; ; ; get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) a into w b, save in b(B2) ; delta position = measured velocity RETURN © 1997 Microchip Technology Inc. DS00532C-page 113 0618 0618 0619 061A 061B 6A8F 0F9B 6A90 119C 061C 0002 061D 061D 9407 061E C627 061F 0620 0621 0622 0623 0624 6A75 4AC1 6A76 4AC2 6A77 4AC3 0625 8C07 0626 8797 0627 0627 0628 0629 062A 062B B800 6A12 190A B561 4A18 02087 02088 02089 02090 02091 02092 02093 02094 02095 02096 M M M M M M 02097 02098 02099 02100 02101 02102 02103 02104 02105 02106 02107 02108 02109 02110 M M M M M M M M 02111 02112 02113 02114 02115 02116 02117 02118 02119 ;***************************************************************************** ;***************************************************************************** ; NAME: doIntegral ; ; DESCRIPTION: Evaluates the integral for the servo calculations. ; doIntegral ADD16 MOVFP ADDWF MOVFP ADDWFC U0,INTEGRAL U0+B0,WREG INTEGRAL+B0, F U0+B1,WREG INTEGRAL+B1, F ; do integral ; ; ; ; get add get add lowest byte lowest byte 2nd byte of 2nd byte of of a into w of b, save in b(B0) a into w b, save in b(B1) RETURN ;***************************************************************************** ;***************************************************************************** ; NAME: doExtstat ; ; DESCRIPTION: Get +limit,-limit,GPI from PORTB and set in EXTSTAT ; doExtstat BTFSS _intir GOTO otherbits MOV24 MPOSITION,INDEXPOS MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF BCF BSF otherbits MOVLB MOVFP RRCF ANDLW MOVPF MPOSITION+B0,WREG WREG,INDEXPOS+B0 MPOSITION+B1,WREG WREG,INDEXPOS+B1 MPOSITION+B2,WREG WREG,INDEXPOS+B2 _intir EXTSTAT,MSB ; ; ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) get byte of a into w move to b(B2) AN532 BANK0 PORTB,WREG WREG, F 0x61 WREG,TMP ; get +limit,-limit and GPI ; arrange in correct bit positions DS00532C-page 114 © 1997 Microchip Technology Inc. AN532 062C 1D18 062D 0818 062E 0997 062F 0002 00000001 02120 02121 02122 02123 02124 02125 02126 02127 02128 02129 02130 02131 02132 02133 02134 02135 02136 02137 02138 02139 02140 02141 02142 02143 02144 02145 02146 02147 02148 02149 02150 02151 02152 02153 02154 02155 02156 02157 02158 02159 02160 02161 02162 02163 02164 02165 02166 SWAPF IORWF IORWF RETURN TMP, F TMP,W EXTSTAT, F ; set in EXTSTAT ;***************************************************************************** ;***************************************************************************** ; NAME: Dmult ; ; DESCRIPTION: Mult: AARG (16 bits) * BARG (16 bits) -> DPX (32 bits) ; ; (a) Load the 1st operand in locations AARG+B0 & AARG+B1 (16 bits) ; (b) Load the 2nd operand in locations BARG+B0 & BARG+B1 (16 bits) ; (c) CALL Dmult ; (d) The 32 bit result is in locations (DPX+B0,DPX+B1,DPX+B2,DPX+B3) ; ; In the signed case, a savings of 9 clks can be realized by choosing ; BARG as the positive factor in the product when possible. ; ; TIMING (worst case): unsigned: 173 clks ; signed: if BARG positive: 170 clks ; if BARG negative: 179 clks ; ;************************************************************************* SIGNED ; Set This To ‘TRUE’ for signed multiply ; and ‘FALSE’ for unsigned. ;************************************************************************* ; Multiplication Macro ;************************************************************************* ; ; TIMING: unsigned: 11+7*10+8*11 = 169 clks ;(worst case) signed: 11+7*10+7*11+5 = 163 clks ; MULTMAC MACRO variable i variable i = 0 #if SIGNED variable MULT_LP_CNT = 15 #else variable MULT_LP_CNT = 16 #endif .while i < MULT_LP_CNT equ TRUE © 1997 Microchip Technology Inc. DS00532C-page 115 02167 02168 02169 02170 02171 02172 02173 02174 02175 02176 02177 02178 02179 02180 02181 02182 02183 02184 02185 02186 02187 02188 02189 02190 02191 02192 02193 02194 02195 add0 02196 02197 02198 02199 02200 02201 02202 02203 02204 02205 02206 02207 02208 02209 02210 02211 02212 02213 .if i < 8 BTFSC BARG+B0,i .else BTFSC BARG+B1,i-8 .fi GOTO add#v(i) .if i < 8 RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F .else RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .endw ; test low byte ; test high byte ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit CLRF RETURN DPX+B0, F ; if we get here, BARG = 0 MOVFP ADDWF MOVFP ADDWFC RLCF RRCF RRCF RRCF variable i = 1 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ;add lsb ;add msb ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 AN532 .while i < MULT_LP_CNT .if i < 8 BTFSS .else BTFSS BARG+B0,i BARG+B1,i-8 ; test low byte ; test high byte DS00532C-page 116 © 1997 Microchip Technology Inc. 0630 AN532 02214 02215 02216 02217 02218 02219 02220 02221 02222 02223 02224 02225 02226 02227 02228 02229 02230 02231 02232 02233 02234 02235 02236 02237 02238 02239 02240 02241 02242 02243 02244 02245 02246 02247 02248 02249 02250 02251 02252 02253 02254 02255 02256 02257 02258 02259 02260 .fi GOTO add#v(i) MOVFP ADDWF MOVFP ADDWFC noadd#v(i) .if i < 8 RLCF RRCF RRCF RRCF .else RLCF RRCF RRCF RRCF RRCF .fi noadd#v(i) AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ;add lsb ;add msb AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit variable i = i+1 .endw #if SIGNED RLCF RRCF RRCF RRCF RRCF #endif ENDM ; ; ; Dmult #if Double Precision Multiply ( 16x16 -> 32 ) ( AARG*BARG -> : 32 bit output in DPX AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; since BARG is always made positive, ; the last bit is known to be zero. SIGNED © 1997 Microchip Technology Inc. DS00532C-page 117 0630 971F 0631 C63C 0632 0633 0634 0635 0636 131C 131D 290A 151C 111D 0637 0638 0639 063A 063B 131E 131F 290A 151E 111F 063C 063C 291B 063D 291A 0000 0000 000F 063E 981E 063F C6A1 0640 0641 0642 0643 1A1B 191B 191A 1919 02261 BTFSS BARG+B1,MSB 02262 GOTO argsok 02263 NEG16 AARG+B0 M M COMF AARG+B0+B0, F M COMF AARG+B0+B1, F M CLRF WREG, F M INCF AARG+B0+B0, F M ADDWFC AARG+B0+B1, F M 02264 NEG16 BARG+B0 M M COMF BARG+B0+B0, F M COMF BARG+B0+B1, F M CLRF WREG, F M INCF BARG+B0+B0, F M ADDWFC BARG+B0+B1, F M 02265 02266 #endif 02267 argsok 02268 CLRF DPX+B3, F 02269 CLRF DPX+B2, F 02270 02271 MULTMAC M variable i M M variable i = 0 M #if SIGNED M variable MULT_LP_CNT = 15 M #else M variable MULT_LP_CNT = 16 M #endif M .while i < MULT_LP_CNT M M .if i < 8 M BTFSC BARG+B0,i M .else M BTFSC BARG+B1,i-8 M .fi M GOTO add0 M .if i < 8 M RLCF DPX+B3,W M RRCF DPX+B3, F M RRCF DPX+B2, F M RRCF DPX+B1, F M .else ; test sign of BARG ; if positive, ok ; if negative, then negate both ; AARG and BARG ; clear initial partial product ; use macro for multiplication ; test low byte ; test high byte AN532 ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 DS00532C-page 118 © 1997 Microchip Technology Inc. AN532 0001 0644 991E 0645 C6AB 0646 0647 0648 0649 1A1B 191B 191A 1919 0002 064A 9A1E 064B C6B5 064C 064D 064E 064F 1A1B 191B 191A 1919 0003 M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .if i < 8 BTFSC BARG+B0,i .else BTFSC BARG+B1,i-8 .fi GOTO add1 .if i < 8 RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F .else RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .if i < 8 BTFSC BARG+B0,i .else BTFSC BARG+B1,i-8 .fi GOTO add2 .if i < 8 RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F .else RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 ; rotate sign into carry bit ; test low byte ; test high byte ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit ; test low byte ; test high byte ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit © 1997 Microchip Technology Inc. DS00532C-page 119 0650 9B1E 0651 C6BF 0652 0653 0654 0655 1A1B 191B 191A 1919 0004 0656 9C1E 0657 C6C9 0658 0659 065A 065B 1A1B 191B 191A 1919 0005 065C 9D1E 065D C6D3 M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M .if i < 8 BTFSC BARG+B0,i .else BTFSC BARG+B1,i-8 .fi GOTO add3 .if i < 8 RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F .else RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .if i < 8 BTFSC BARG+B0,i .else BTFSC BARG+B1,i-8 .fi GOTO add4 .if i < 8 RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F .else RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .if i < 8 BTFSC .else BTFSC .fi GOTO ; test low byte ; test high byte ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit ; test low byte ; test high byte ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit AN532 BARG+B0,i BARG+B1,i-8 add5 ; test low byte ; test high byte DS00532C-page 120 © 1997 Microchip Technology Inc. AN532 065E 065F 0660 0661 1A1B 191B 191A 1919 0006 0662 9E1E 0663 C6DD 0664 0665 0666 0667 1A1B 191B 191A 1919 0007 0668 9F1E 0669 C6E7 066A 066B 066C 066D 1A1B 191B 191A 1919 M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M .if i < 8 RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F .else RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .if i < 8 BTFSC BARG+B0,i .else BTFSC BARG+B1,i-8 .fi GOTO add6 .if i < 8 RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F .else RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .if i < 8 BTFSC .else BTFSC .fi GOTO .if i < 8 RLCF RRCF RRCF RRCF .else RLCF ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit ; test low byte ; test high byte ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit BARG+B0,i BARG+B1,i-8 add7 DPX+B3,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B3,W ; test low byte ; test high byte ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit © 1997 Microchip Technology Inc. DS00532C-page 121 0008 066E 981F 066F C6F1 0670 0671 0672 0673 0674 1A1B 191B 191A 1919 1918 0009 0675 991F 0676 C6FC 0677 0678 0679 067A 067B 1A1B 191B 191A 1919 1918 000A M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .if i < 8 BTFSC BARG+B0,i .else BTFSC BARG+B1,i-8 .fi GOTO add8 .if i < 8 RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F .else RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .if i < 8 BTFSC BARG+B0,i .else BTFSC BARG+B1,i-8 .fi GOTO add9 .if i < 8 RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F .else RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 ; test low byte ; test high byte ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit ; test low byte ; test high byte ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit AN532 DS00532C-page 122 © 1997 Microchip Technology Inc. AN532 067C 9A1F 067D C707 067E 067F 0680 0681 0682 1A1B 191B 191A 1919 1918 000B 0683 9B1F 0684 C712 0685 0686 0687 0688 0689 1A1B 191B 191A 1919 1918 000C 068A 9C1F 068B C71D M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M .if i < 8 BTFSC BARG+B0,i .else BTFSC BARG+B1,i-8 .fi GOTO add10 .if i < 8 RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F .else RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .if i < 8 BTFSC BARG+B0,i .else BTFSC BARG+B1,i-8 .fi GOTO add11 .if i < 8 RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F .else RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .if i < 8 BTFSC .else BTFSC .fi GOTO .if i < 8 ; test low byte ; test high byte ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit ; test low byte ; test high byte ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit BARG+B0,i BARG+B1,i-8 add12 ; test low byte ; test high byte © 1997 Microchip Technology Inc. DS00532C-page 123 068C 068D 068E 068F 0690 1A1B 191B 191A 1919 1918 000D 0691 9D1F 0692 C728 0693 0694 0695 0696 0697 1A1B 191B 191A 1919 1918 000E 0698 9E1F 0699 C733 069A 1A1B 069B 191B M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F .else RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .if i < 8 BTFSC BARG+B0,i .else BTFSC BARG+B1,i-8 .fi GOTO add13 .if i < 8 RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F .else RLCF DPX+B3,W RRCF DPX+B3, F RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .if i < 8 BTFSC .else BTFSC .fi GOTO .if i < 8 RLCF RRCF RRCF RRCF .else RLCF RRCF ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit ; test low byte ; test high byte ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit BARG+B0,i BARG+B1,i-8 add14 ; test low byte ; test high byte AN532 DPX+B3,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B3,W DPX+B3, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 ; rotate sign into carry bit DS00532C-page 124 © 1997 Microchip Technology Inc. AN532 069C 191A 069D 1919 069E 1918 000F 069F 2918 06A0 0002 06A1 06A1 06A2 06A3 06A4 06A5 06A6 06A7 06A8 6A1C 0F1A 6A1D 111B 1A1D 191B 191A 1919 0001 06A9 911E 06AA 06AB 06AB 06AC 06AD 06AE 06AF C6AF 6A1C 0F1A 6A1D 111B 06AF 06B0 06B1 06B2 1A1D 191B 191A 1919 M M M M M M M M M M M M M M add0 M M M M M M M M M M M M M M M M M M M M M add1 M M M M M M noadd1 M M M M M M RRCF DPX+B2, F RRCF DPX+B1, F RRCF DPX+B0, F .fi variable i = i+1 .endw CLRF RETURN DPX+B0, F ; if we get here, BARG = 0 MOVFP ADDWF MOVFP ADDWFC RLCF RRCF RRCF RRCF variable i = 1 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ;add lsb ;add msb ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 .while i < MULT_LP_CNT .if i < 8 BTFSS .else BTFSS .fi GOTO MOVFP ADDWF MOVFP ADDWFC BARG+B0,i BARG+B1,i-8 noadd1 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ; test low byte ; test high byte ; add lsb ; add msb .if i < 8 RLCF RRCF RRCF RRCF AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 © 1997 Microchip Technology Inc. DS00532C-page 125 0002 06B3 921E 06B4 06B5 06B5 06B6 06B7 06B8 06B9 C6B9 6A1C 0F1A 6A1D 111B 06B9 06BA 06BB 06BC 1A1D 191B 191A 1919 0003 06BD 931E M M M M M M M M M M M M M M M M M M M M add2 M M M M M M noadd2 M M M M M M M M M M M M M M M M M M M M M .else RLCF RRCF RRCF RRCF RRCF .fi variable i = i+1 .if i < 8 BTFSS .else BTFSS .fi GOTO MOVFP ADDWF MOVFP ADDWFC AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit BARG+B0,i BARG+B1,i-8 noadd2 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ; test low byte ; test high byte ;add lsb ;add msb .if i < 8 RLCF RRCF RRCF RRCF .else RLCF RRCF RRCF RRCF RRCF .fi variable i = i+1 .if i < 8 BTFSS AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 AN532 BARG+B0,i ; test low byte DS00532C-page 126 © 1997 Microchip Technology Inc. AN532 06BE 06BF 06BF 06C0 06C1 06C2 06C3 C6C3 6A1C 0F1A 6A1D 111B 06C3 06C4 06C5 06C6 1A1D 191B 191A 1919 0004 06C7 941E 06C8 06C9 06C9 06CA 06CB 06CC 06CD C6CD 6A1C 0F1A 6A1D 111B 06CD 1A1D 06CE 191B M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M .else BTFSS .fi GOTO add3 MOVFP ADDWF MOVFP ADDWFC noadd3 .if i < 8 RLCF RRCF RRCF RRCF .else RLCF RRCF RRCF RRCF RRCF .fi BARG+B1,i-8 noadd3 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ; test high byte ;add lsb ;add msb AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit variable i = i+1 .if i < 8 BTFSS .else BTFSS .fi GOTO add4 MOVFP ADDWF MOVFP ADDWFC noadd4 .if i < 8 RLCF RRCF AARG+B1,W DPX+B3, F ; rotate sign into carry bit ; for i < 8, no meaningful bits AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ;add lsb ;add msb BARG+B0,i BARG+B1,i-8 noadd4 ; test low byte ; test high byte © 1997 Microchip Technology Inc. DS00532C-page 127 06CF 191A 06D0 1919 0005 06D1 951E 06D2 06D3 06D3 06D4 06D5 06D6 06D7 C6D7 6A1C 0F1A 6A1D 111B 06D7 06D8 06D9 06DA 1A1D 191B 191A 1919 0006 M M M M M M M M M M M M M M M M M M M M M M add5 M M M M M M noadd5 M M M M M M M M M M M M M M M M M M M RRCF RRCF .else RLCF RRCF RRCF RRCF RRCF .fi DPX+B2, F DPX+B1, F ; are in DPX+B0 AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit variable i = i+1 .if i < 8 BTFSS .else BTFSS .fi GOTO MOVFP ADDWF MOVFP ADDWFC BARG+B0,i BARG+B1,i-8 noadd5 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ; test low byte ; test high byte ;add lsb ;add msb .if i < 8 RLCF RRCF RRCF RRCF .else RLCF RRCF RRCF RRCF RRCF .fi variable i = i+1 AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 AN532 DS00532C-page 128 © 1997 Microchip Technology Inc. AN532 06DB 961E 06DC 06DD 06DD 06DE 06DF 06E0 06E1 C6E1 6A1C 0F1A 6A1D 111B 06E1 06E2 06E3 06E4 1A1D 191B 191A 1919 0007 06E5 971E 06E6 06E7 06E7 06E8 06E9 06EA 06EB C6EB 6A1C 0F1A 6A1D 111B M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M .if i < 8 BTFSS .else BTFSS .fi GOTO add6 MOVFP ADDWF MOVFP ADDWFC noadd6 .if i < 8 RLCF RRCF RRCF RRCF .else RLCF RRCF RRCF RRCF RRCF .fi BARG+B0,i BARG+B1,i-8 noadd6 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ; test low byte ; test high byte ;add lsb ;add msb AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit variable i = i+1 .if i < 8 BTFSS .else BTFSS .fi GOTO add7 MOVFP ADDWF MOVFP ADDWFC noadd7 .if i < 8 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ;add lsb ;add msb BARG+B0,i BARG+B1,i-8 noadd7 ; test low byte ; test high byte © 1997 Microchip Technology Inc. DS00532C-page 129 06EB 06EC 06ED 06EE 1A1D 191B 191A 1919 0008 06EF 901F 06F0 06F1 06F1 06F2 06F3 06F4 06F5 C6F5 6A1C 0F1A 6A1D 111B 06F5 06F6 06F7 06F8 06F9 1A1D 191B 191A 1919 1918 M M M M M M M M M M M M M M M M M M M M M M M M add8 M M M M M M noadd8 M M M M M M M M M M M M M M M M M RLCF RRCF RRCF RRCF .else RLCF RRCF RRCF RRCF RRCF .fi AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit variable i = i+1 .if i < 8 BTFSS .else BTFSS .fi GOTO MOVFP ADDWF MOVFP ADDWFC BARG+B0,i BARG+B1,i-8 noadd8 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ; test low byte ; test high byte ;add lsb ;add msb .if i < 8 RLCF RRCF RRCF RRCF .else RLCF RRCF RRCF RRCF RRCF .fi AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 AN532 DS00532C-page 130 © 1997 Microchip Technology Inc. AN532 0009 06FA 911F 06FB 06FC 06FC 06FD 06FE 06FF 0700 C700 6A1C 0F1A 6A1D 111B 0700 0701 0702 0703 0704 1A1D 191B 191A 1919 1918 000A 0705 921F 0706 0707 0707 0708 0709 070A 070B C70B 6A1C 0F1A 6A1D 111B M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M M variable i = i+1 .if i < 8 BTFSS .else BTFSS .fi GOTO add9 MOVFP ADDWF MOVFP ADDWFC noadd9 .if i < 8 RLCF RRCF RRCF RRCF .else RLCF RRCF RRCF RRCF RRCF .fi variable i = i+1 .if i < 8 BTFSS .else BTFSS .fi GOTO add10 MOVFP ADDWF MOVFP ADDWFC noadd10 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ;add lsb ;add msb AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ;add lsb ;add msb BARG+B0,i BARG+B1,i-8 noadd9 ; test low byte ; test high byte BARG+B0,i BARG+B1,i-8 noadd10 ; test low byte ; test high byte © 1997 Microchip Technology Inc. DS00532C-page 131 070B 070C 070D 070E 070F 1A1D 191B 191A 1919 1918 000B 0710 931F 0711 0712 0712 0713 0714 0715 0716 C716 6A1C 0F1A 6A1D 111B 0716 0717 0718 0719 071A 1A1D 191B 191A 1919 1918 M M M M M M M M M M M M M M M M M M M M M M M M M M add11 M M M M M M noadd11 M M M M M M M M M M M M M M M .if i < 8 RLCF RRCF RRCF RRCF .else RLCF RRCF RRCF RRCF RRCF .fi variable i = i+1 .if i < 8 BTFSS .else BTFSS .fi GOTO MOVFP ADDWF MOVFP ADDWFC AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 BARG+B0,i BARG+B1,i-8 noadd11 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ; test low byte ; test high byte ; add lsb ; add msb .if i < 8 RLCF RRCF RRCF RRCF .else AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 AN532 RLCF RRCF RRCF RRCF RRCF AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit DS00532C-page 132 © 1997 Microchip Technology Inc. AN532 000C 071B 941F 071C 071D 071D 071E 071F 0720 0721 C721 6A1C 0F1A 6A1D 111B 0721 0722 0723 0724 0725 1A1D 191B 191A 1919 1918 000D 0726 951F 0727 0728 0728 0729 072A 072B C72C 6A1C 0F1A 6A1D 111B M M M M M M M M M M M add12 M M M M M M noadd12 M M M M M M M M M M M M M M M M M M M M M M M M M M add13 M M M M .fi variable i = i+1 .if i < 8 BTFSS .else BTFSS .fi GOTO MOVFP ADDWF MOVFP ADDWFC BARG+B0,i BARG+B1,i-8 noadd12 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ; test low byte ; test high byte ; add lsb ; add msb .if i < 8 RLCF RRCF RRCF RRCF .else RLCF RRCF RRCF RRCF RRCF .fi variable i = i+1 .if i < 8 BTFSS .else BTFSS .fi GOTO MOVFP ADDWF MOVFP ADDWFC AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 BARG+B0,i BARG+B1,i-8 noadd13 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ; test low byte ; test high byte ; add lsb ; add msb © 1997 Microchip Technology Inc. DS00532C-page 133 072C 072C 072D 072E 072F 0730 1A1D 191B 191A 1919 1918 000E 0731 961F 0732 0733 0733 0734 0735 0736 0737 C737 6A1C 0F1A 6A1D 111B 0737 0738 0739 073A 1A1D 191B 191A 1919 M M noadd13 M M M M M M M M M M M M M M M M M M M M M M M M M M add14 M M M M M M noadd14 M M M M M M M M M M M M M .if i < 8 RLCF RRCF RRCF RRCF .else RLCF RRCF RRCF RRCF RRCF .fi variable i = i+1 .if i < 8 BTFSS .else BTFSS .fi GOTO MOVFP ADDWF MOVFP ADDWFC AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; rotate sign into carry bit AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 BARG+B0,i BARG+B1,i-8 noadd14 AARG+B0,WREG DPX+B2, F AARG+B1,WREG DPX+B3, F ; test low byte ; test high byte ;add lsb ;add msb .if i < 8 RLCF RRCF RRCF RRCF .else RLCF RRCF RRCF RRCF AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 AN532 DS00532C-page 134 © 1997 Microchip Technology Inc. AN532 073B 1918 000F 073C 073D 073E 073F 0740 1A1D 191B 191A 1919 1918 0741 0002 0742 0742 B000 0743 010D M M M M M M M M M M M M M M M M M 02272 02273 02274 02275 02276 02277 02278 02279 02280 02281 02282 02283 02284 02285 02286 02287 02288 02289 02290 02291 02292 02293 02294 02295 02296 02297 02298 02299 02300 02301 RRCF .fi DPX+B0, F variable i = i+1 .endw #if SIGNED RLCF RRCF RRCF RRCF RRCF #endif AARG+B1,W DPX+B3, F DPX+B2, F DPX+B1, F DPX+B0, F ; since BARG is always made positive, ; the last bit is known to be zero. RETURN ;***************************************************************************** ; ;***************************************************************************** ; NAME: doCaptureRegs ; ; DESCRIPTION: Captures Desired Register Values To PIC-MASTER Trace Buffer ; Intended for PICMASTER Demo/debug/servo tuning Purposes Only ; Capture The following registers to Trace Buffer by putting ; A Trace point on a TABLW instruction. Trace only 2nd Cycle ; ; (a) POSERROR (position error : 16 bits) ; (b) VELERROR (velocity error : 16 bits) ; (c) MPOSITION (measured position value : 24 bits) ; (d) MVELOCITY (measured velocity value : 24 bits) ; (e) POSITION (commanded position : 24 bits) ; (f) VELOCITY (commanded velocity : 24 bits) ; (g) Y (output of servo loop : 32 bits) ; (h) YPWM (output value written to PWM : 10 bits) ; ; #define CaptureAddr 0x8000 ; doCaptureRegs ; !end! hdr !skip start! movlw (CaptureAddr & 0xff) movwf TBLPTRL © 1997 Microchip Technology Inc. DS00532C-page 135 0744 B080 0745 010E 0746 AC7C 0747 A67D 0748 0748 AC7C 0749 AC7F 074A A680 074B 074B AC7F 074C AC75 074D A676 074E 074E AC75 074F AC58 0750 A659 0751 0751 AC58 0752 AC78 0753 A679 0754 0754 AC78 0755 AC5B 0756 A65C 0757 0757 AC5B 0758 290A 0759 07C8 075A 03C9 075B 075C 075D 075E 075F 6AC8 08C9 330A 0002 29C5 02302 02303 02304 02305 02306 02307 02308 02309 02310 02311 02312 02313 02314 02315 02316 02317 02318 02319 02320 02321 02322 02323 02324 02325 02326 02327 02328 02329 02330 02331 02332 02333 02334 02335 M M M M M 02336 M M M M 02337 02338 02339 movlw movwf tablwt tlwt capPerr tablwt tablwt tlwt capVerr tablwt tablwt tlwt capMpos tablwt tablwt tlwt capPos tablwt tablwt tlwt capMvel tablwt tablwt tlwt capVel tablwt DEC16 CLRF DECF SUBWFB TFSZ16 MOVFP IORWF TSTFSZ RETURN CLRF MOV16 CaptureAddr/256 TBLPTRH ; setup table pointer address 0,0,POSERROR+B0 ; dummy tablwt 1,POSERROR+B1 ; now table latch = 16 bits contents of POSERROR 0,0,POSERROR+B0 ; perform actual table write of POSERROR 0,0,VELERROR+B0 1,VELERROR+B1 0,0,VELERROR+B0 0,0,MPOSITION+B0 1,MPOSITION+B1 0,0,MPOSITION+B0 0,0,POSITION+B0 1,POSITION+B1 0,0,POSITION+B0 0,0,MVELOCITY+B0 1,MVELOCITY+B1 0,0,MVELOCITY+B0 0,0,VELOCITY+B0 1,VELOCITY+B1 0,0,VELOCITY+B0 CAPTMP WREG, F CAPTMP+B0, F CAPTMP+B1, F CAPTMP ; capture Velocity error ; capture measured position ; capture commanded position ; capture measured velocity ; capture commanded velocity AN532 CAPTMP+B0,WREG CAPTMP+B1,W WREG CAPFLAG, F CAPCOUNT,CAPTMP DS00532C-page 136 © 1997 Microchip Technology Inc. AN532 0760 0761 0762 0763 6AC6 4AC8 6AC7 4AC9 0764 0001 0765 0002 0766 0766 000D 0767 00A6 0768 004D 0769 00A8 076A 004F 076B 00B6 076C 0053 076D 00D2 076E 0052 076F 00F9 0770 0043 0771 0123 0772 0050 0773 0145 M M M M M M 02340 02341 02342 02343 02344 02345 02346 02347 02348 02349 02350 M M 02351 M M M 02352 M M M 02353 M M M 02354 M M M 02355 M M M 02356 M M M 02357 M M M MOVFP MOVPF MOVFP MOVPF HALT RETURN CAPCOUNT+B0,WREG WREG,CAPTMP+B0 CAPCOUNT+B1,WREG WREG,CAPTMP+B1 ; ; ; ; get byte of a into w move to b(B0) get byte of a into w move to b(B1) ;***************************************************************************** ; ; ; TABLES: CMD_START CMD_TABLE CMD_TABLE CMD_DEF do_null,DO_NULL DATA DO_NULL DATA do_null CMD_DEF do_move,DO_MOVE DATA DO_MOVE DATA do_move CMD_DEF do_mode,DO_MODE DATA DO_MODE DATA do_mode CMD_DEF do_setparameter,DO_SETPARAMETER DATA DO_SETPARAMETER DATA do_setparameter CMD_DEF do_readparameter,DO_READPARAMETER DATA DO_READPARAMETER DATA do_readparameter CMD_DEF do_shutter,DO_SHUTTER DATA DO_SHUTTER DATA do_shutter CMD_DEF do_readcomposition,DO_READCOMPOSITION DATA DATA DO_READCOMPOSITION do_readcomposition © 1997 Microchip Technology Inc. DS00532C-page 137 0774 0056 0775 014E 0776 0070 0777 0157 0778 0076 0779 0160 077A 0058 077B 0169 077C 0059 077D 0170 077E 0049 077F 0174 0780 0048 0781 017D 0782 005A 0783 0190 0784 0073 0785 0193 0786 0063 0787 0196 0788 0000 0789 0003 078A 0020 02358 CMD_DEF M M DATA M DATA 02359 CMD_DEF M M DATA M DATA 02360 CMD_DEF M M DATA M DATA 02361 CMD_DEF M M DATA M DATA 02362 CMD_DEF M M DATA M DATA 02363 CMD_DEF M M DATA M DATA 02364 CMD_DEF M M DATA M DATA 02365 CMD_DEF M M DATA M DATA 02366 CMD_DEF M M DATA M DATA 02367 CMD_DEF M M DATA M DATA 02368 CMD_END M ; M DATA 02369 02370 02371 PAR_TABLE 02372 do_readcomvelocity,DO_READCOMVELOCITY DO_READCOMVELOCITY do_readcomvelocity do_readactposition,DO_READACTPOSITION DO_READACTPOSITION do_readactposition do_readactvelocity,DO_READACTVELOCITY DO_READACTVELOCITY do_readactvelocity do_externalstatus,DO_EXTERNALSTATUS DO_EXTERNALSTATUS do_externalstatus do_movestatus,DO_MOVESTATUS DO_MOVESTATUS do_movestatus do_readindposition,DO_READINDPOSITION DO_READINDPOSITION do_readindposition do_setposition,DO_SETPOSITION DO_SETPOSITION do_setposition do_reset,DO_RESET DO_RESET do_reset do_stop,DO_STOP DO_STOP do_stop do_capture,DO_CAPTURE DO_CAPTURE do_capture AN532 ; 0x00 DATA DATA 0x0003 VL DS00532C-page 138 © 1997 Microchip Technology Inc. AN532 078B 078C 078D 078E 078F 0790 0791 0792 0793 0794 0795 0796 0797 0798 0799 02373 DATA 02374 DATA 02375 DATA 02376 DATA 02377 DATA 02378 DATA 02379 DATA 02380 DATA 02381 DATA 02382 DATA 02383 DATA 02384 DATA 02385 DATA 02386 DATA 02387 DATA 02388 02389 #if DECIO 02390 079A 423F 02391 DEC_TABLE DATA 079B 000F 02392 DATA 079C 869F 02393 DATA 079D 0001 02394 DATA 079E 270F 02395 DATA 079F 0000 02396 DATA 07A0 03E7 02397 DATA 07A1 0000 02398 DATA 07A2 0063 02399 DATA 07A3 0000 02400 DATA 07A4 0009 02401 DATA 07A5 0000 02402 DATA 07A6 FFFF 02403 DATA 02404 #endif 02405 02406 02407 02408 02409 END MEMORY USAGE MAP (‘X’ = Used, ‘-’ = Unused) 0000 0040 0080 00C0 0100 0140 0180 01C0 : : : : : : : : X--------------XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX ---------------XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX 0103 0023 0202 0026 0302 0028 0402 002A 0501 002C 0602 002D 0702 002F 0008 0x0103 AL 0x0202 KP 0x0302 KV 0x0402 KI 0x0501 IM 0x0602 FV 0x0702 FA NUMPAR 0x423F 0x000F 0x869F 0x0001 0x270F 0x0000 0x03E7 0x0000 0x0063 0x0000 0x0009 0x0000 0xFFFF XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX © 1997 Microchip Technology Inc. DS00532C-page 139 0200 0240 0280 02C0 0300 0340 0380 03C0 0400 0440 0480 04C0 0500 0540 0580 05C0 0600 0640 0680 06C0 0700 0740 0780 : : : : : : : : : : : : : : : : : : : : : : : XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXX--------- XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX XXXXXXXXXXXXXXXX ---------------- All other memory blocks unused. Program Memory Words Used: Errors : Warnings : Messages : 0 0 reported, 0 reported, 1928 0 suppressed 0 suppressed AN532 Note the following details of the code protection feature on PICmicro® MCUs. • • • The PICmicro family meets the specifications contained in the Microchip Data Sheet. Microchip believes that its family of PICmicro microcontrollers is one of the most secure products of its kind on the market today, when used in the intended manner and under normal conditions. There are dishonest and possibly illegal methods used to breach the code protection feature. All of these methods, to our knowledge, require using the PICmicro microcontroller in a manner outside the operating specifications contained in the data sheet. The person doing so may be engaged in theft of intellectual property. Microchip is willing to work with the customer who is concerned about the integrity of their code. Neither Microchip nor any other semiconductor manufacturer can guarantee the security of their code. Code protection does not mean that we are guaranteeing the product as “unbreakable”. Code protection is constantly evolving. We at Microchip are committed to continuously improving the code protection features of our product. • • • If you have any further questions about this matter, please contact the local sales office nearest to you. Information contained in this publication regarding device applications and the like is intended through suggestion only and may be superseded by updates. It is your responsibility to ensure that your application meets with your specifications. No representation or warranty is given and no liability is assumed by Microchip Technology Incorporated with respect to the accuracy or use of such information, or infringement of patents or other intellectual property rights arising from such use or otherwise. Use of Microchip’s products as critical components in life support systems is not authorized except with express written approval by Microchip. No licenses are conveyed, implicitly or otherwise, under any intellectual property rights. Trademarks The Microchip name and logo, the Microchip logo, FilterLab, KEELOQ, microID, MPLAB, PIC, PICmicro, PICMASTER, PICSTART, PRO MATE, SEEVAL and The Embedded Control Solutions Company are registered trademarks of Microchip Technology Incorporated in the U.S.A. and other countries. dsPIC, ECONOMONITOR, FanSense, FlexROM, fuzzyLAB, In-Circuit Serial Programming, ICSP, ICEPIC, microPort, Migratable Memory, MPASM, MPLIB, MPLINK, MPSIM, MXDEV, PICC, PICDEM, PICDEM.net, rfPIC, Select Mode and Total Endurance are trademarks of Microchip Technology Incorporated in the U.S.A. Serialized Quick Turn Programming (SQTP) is a service mark of Microchip Technology Incorporated in the U.S.A. All other trademarks mentioned herein are property of their respective companies. © 2002, Microchip Technology Incorporated, Printed in the U.S.A., All Rights Reserved. Printed on recycled paper. Microchip received QS-9000 quality system certification for its worldwide headquarters, design and wafer fabrication facilities in Chandler and Tempe, Arizona in July 1999. The Company’s quality system processes and procedures are QS-9000 compliant for its PICmicro® 8-bit MCUs, KEELOQ® code hopping devices, Serial EEPROMs and microperipheral products. In addition, Microchip’s quality system for the design and manufacture of development systems is ISO 9001 certified.  2002 Microchip Technology Inc. M WORLDWIDE SALES AND SERVICE AMERICAS Corporate Office 2355 West Chandler Blvd. Chandler, AZ 85224-6199 Tel: 480-792-7200 Fax: 480-792-7277 Technical Support: 480-792-7627 Web Address: http://www.microchip.com ASIA/PACIFIC Australia Microchip Technology Australia Pty Ltd Suite 22, 41 Rawson Street Epping 2121, NSW Australia Tel: 61-2-9868-6733 Fax: 61-2-9868-6755 Japan Microchip Technology Japan K.K. Benex S-1 6F 3-18-20, Shinyokohama Kohoku-Ku, Yokohama-shi Kanagawa, 222-0033, Japan Tel: 81-45-471- 6166 Fax: 81-45-471-6122 Rocky Mountain 2355 West Chandler Blvd. Chandler, AZ 85224-6199 Tel: 480-792-7966 Fax: 480-792-7456 China - Beijing Microchip Technology Consulting (Shanghai) Co., Ltd., Beijing Liaison Office Unit 915 Bei Hai Wan Tai Bldg. No. 6 Chaoyangmen Beidajie Beijing, 100027, No. China Tel: 86-10-85282100 Fax: 86-10-85282104 Korea Microchip Technology Korea 168-1, Youngbo Bldg. 3 Floor Samsung-Dong, Kangnam-Ku Seoul, Korea 135-882 Tel: 82-2-554-7200 Fax: 82-2-558-5934 Atlanta 500 Sugar Mill Road, Suite 200B Atlanta, GA 30350 Tel: 770-640-0034 Fax: 770-640-0307 Singapore Microchip Technology Singapore Pte Ltd. 200 Middle Road #07-02 Prime Centre Singapore, 188980 Tel: 65-6334-8870 Fax: 65-6334-8850 Boston 2 Lan Drive, Suite 120 Westford, MA 01886 Tel: 978-692-3848 Fax: 978-692-3821 China - Chengdu Microchip Technology Consulting (Shanghai) Co., Ltd., Chengdu Liaison Office Rm. 2401, 24th Floor, Ming Xing Financial Tower No. 88 TIDU Street Chengdu 610016, China Tel: 86-28-6766200 Fax: 86-28-6766599 Taiwan Microchip Technology Taiwan 11F-3, No. 207 Tung Hua North Road Taipei, 105, Taiwan Tel: 886-2-2717-7175 Fax: 886-2-2545-0139 Chicago 333 Pierce Road, Suite 180 Itasca, IL 60143 Tel: 630-285-0071 Fax: 630-285-0075 Dallas 4570 Westgrove Drive, Suite 160 Addison, TX 75001 Tel: 972-818-7423 Fax: 972-818-2924 China - Fuzhou Microchip Technology Consulting (Shanghai) Co., Ltd., Fuzhou Liaison Office Unit 28F, World Trade Plaza No. 71 Wusi Road Fuzhou 350001, China Tel: 86-591-7503506 Fax: 86-591-7503521 EUROPE Denmark Microchip Technology Nordic ApS Regus Business Centre Lautrup hoj 1-3 Ballerup DK-2750 Denmark Tel: 45 4420 9895 Fax: 45 4420 9910 Detroit Tri-Atria Office Building 32255 Northwestern Highway, Suite 190 Farmington Hills, MI 48334 Tel: 248-538-2250 Fax: 248-538-2260 China - Shanghai Microchip Technology Consulting (Shanghai) Co., Ltd. Room 701, Bldg. B Far East International Plaza No. 317 Xian Xia Road Shanghai, 200051 Tel: 86-21-6275-5700 Fax: 86-21-6275-5060 Kokomo 2767 S. Albright Road Kokomo, Indiana 46902 Tel: 765-864-8360 Fax: 765-864-8387 France Microchip Technology SARL Parc d’Activite du Moulin de Massy 43 Rue du Saule Trapu Batiment A - ler Etage 91300 Massy, France Tel: 33-1-69-53-63-20 Fax: 33-1-69-30-90-79 Los Angeles 18201 Von Karman, Suite 1090 Irvine, CA 92612 Tel: 949-263-1888 Fax: 949-263-1338 China - Shenzhen Microchip Technology Consulting (Shanghai) Co., Ltd., Shenzhen Liaison Office Rm. 1315, 13/F, Shenzhen Kerry Centre, Renminnan Lu Shenzhen 518001, China Tel: 86-755-2350361 Fax: 86-755-2366086 New York 150 Motor Parkway, Suite 202 Hauppauge, NY 11788 Tel: 631-273-5305 Fax: 631-273-5335 Germany Microchip Technology GmbH Gustav-Heinemann Ring 125 D-81739 Munich, Germany Tel: 49-89-627-144 0 Fax: 49-89-627-144-44 San Jose Microchip Technology Inc. 2107 North First Street, Suite 590 San Jose, CA 95131 Tel: 408-436-7950 Fax: 408-436-7955 Hong Kong Microchip Technology Hongkong Ltd. Unit 901-6, Tower 2, Metroplaza 223 Hing Fong Road Kwai Fong, N.T., Hong Kong Tel: 852-2401-1200 Fax: 852-2401-3431 Italy Microchip Technology SRL Centro Direzionale Colleoni Palazzo Taurus 1 V. Le Colleoni 1 20041 Agrate Brianza Milan, Italy Tel: 39-039-65791-1 Fax: 39-039-6899883 Toronto 6285 Northam Drive, Suite 108 Mississauga, Ontario L4V 1X5, Canada Tel: 905-673-0699 Fax: 905-673-6509 India Microchip Technology Inc. India Liaison Office Divyasree Chambers 1 Floor, Wing A (A3/A4) No. 11, O’Shaugnessey Road Bangalore, 560 025, India Tel: 91-80-2290061 Fax: 91-80-2290062 United Kingdom Arizona Microchip Technology Ltd. 505 Eskdale Road Winnersh Triangle Wokingham Berkshire, England RG41 5TU Tel: 44 118 921 5869 Fax: 44-118 921-5820 03/01/02  2002 Microchip Technology Inc.