Electronica - Servo Control of A Dc-Brush Motor An532

Servo Control of a DC-Brush Motor
AN532
Author: Tim Bucella, Teknic Inc.
INTRODUCTION
The PIC17C42 microcontroller is an excellent choice for cost-effective servo control in embedded applications. Due to its Harvard architecture and RISC-like 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 of the PIC17C42 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. 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 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 offering a number of advantages: Increased Acceleration, Velocity Improved Efficiency Reduced Audible Noise True Disturbance Rejection
FIGURE 1 - A TYPICAL SERVO SYSTEM

Motor Digital Command Processor D/A Power Amplifier Encoder Counter 1 2 M

E Encoder
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 the simple PID algorithm (implemented in this application note). The combination of servo compensator and trajectory calculations can place significant demands on the processor. The digital-to-analog conversion can be handled by a conventional DAC or by using 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 of its simplicity and reduced cost. 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 1 and 2 represents an increment of position. The phase of the signals is used to determine direction of rotation.
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 digital-to-analog converter, and a power amplifier, which delivers current or voltage to the motor.
1993 Microchip Technology Inc.
DS00532B-page 1
4-185

FIGURE 2 - THE PIC17C42 SERVO SYSTEM
+5 Vm
Serial Command Non-Servo I/O
Rx Tx
LMD18201
PWM1 PWM2 TCLK12 Dir +5 Out1 PWM Out2 Brake GND
M
RTCC TCLK3 TCLK12 RX, TX = = = = 16-bit timer input 16-bit timer input 8-bit timer input serial port receive and transmit pins
PIC17C42
RTCC TCLK3
CountUp CountDn
16R8 PLD
1 2
16MHz Osc.
CLKIN CLKOUT
FIGURE 3 - THE PIC17C42 BASED SERVO CONTROL BOARD
Place Servo Control Board photo here.
FIGURE 4 - DIGITAL PID IMPLEMENTATION

Optional Anti-Windup Logic + U(k) +
Z -1
I
+ +
P
+
Y(k)
Saturation
To PWM
Z -1 Z
Note: The Z-1 operator indicates a one sample time delay
DS00532B-page 2
4-186

These encoder signals are usually decoded using a small state machine into Count Up and Count Down pulses. These pulses are then routed to an N-bit, updown 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. 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 3. 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. 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 digital-to-analog 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 (Figures 5 and 6).
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.5nS (at 16 MHz). This translates into 10-bit resolution at a 15.6KHz rate or 1 part in 800 (9 1/2-bit) resolution at 20KHz. 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.5mS or more and a mechanical time constant of 20.0mS or more. A 15KHz PWM output is effectively equivalent to that of a linear amplifier. In the system shown in Figure 2, the H-bridges direction input is wired directly to the PIC17C42s 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.
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 2. The PLD converts the quadrature pulses into two pulse streams: Count Up and Count Down (Figure 5). These signals are then fed to two 16-bit timers of the PIC17C42 (TMR3 and RTCC). A logic description for the PLD decoder is shown in Appendix B. The PIC17C42 keeps track of the motor shafts 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 deep, 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 1mS, the maximum encoder rate would be 3.2767 MHz. Counter wrap-around is not a concern because only the difference between the two counters is used. Twoscomplement 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.
THE COMPENSATOR
PID is the most widely used algorithm for servo motor control. Although it may not be the most optimum controller for all applications, however it is easy to understand and tune. The standard digital PID algorithms form is shown in Figure 4. U(k) is the position or velocity error and Y(k) is the output. This algorithm has been implemented using the PIC17C42 math library. Only 800 instruction cycles are required resulting in a 0.2mS PID execution time at 16 MHz. Integrator wind-up 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 above avoids this problem by stopping the action of the integrator during output saturation.
DS00532B-page 3
4-187

FIGURE 5 - SEQUENTIAL ENCODER SIGNALS
1 1 2 2
1x mode up_count
down_count 4x mode up_count
down_count
FIGURE 6 - ENCODER INTERFACE SCHEME
RTCC up-count RA1/RT 16-bit counter PLD 16R8 TMR3 down-count RB5/TCLK3 16-bit counter PIC17C42 19 5 2 18 4 1
1 1x/4x select
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 short move where maximum velocity was not reached (Figure 8). 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.
DS00532B-page 4
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 (TMR2) 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 externallyclocked, 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 7).
4-188

FIGURE 7 - SAMPLING SCHEME
Timer 1 <8> PWM output 15.625 KHz
PWM1 PWM2 Reset Timer 2 <8>
Comparator Servo-update interrupt 0.9765625 KHz
Period-reg 2 Period=16 PIC17C42
4
16 PWM cycles
16 PWM cycles = 1.024ms
1 PWM cycle = 64s Servo-update interrupt
FIGURE 8 - VELOCITY RAMP SEGMENTS FOR POSITION MOVES

Velocity
Velocity Limit Short Move Slope = Accel. limit Long Move
Time
DS00532B-page 5
4-189

FIGURE 9 - FLOWCHART FOR FOREGROUND PROCESSING FIGURE 10 - FLOWCHART FOR INTERRUPT SERVICE ROUTINE
DCMOTOR.ASM
IntPoll
Program Setup
Save Registers
IdleFunction
doMposMvel Read up_count and down_count Measure current velocity, position
doExtStat Monitor ext status inputs
GetChk
Move Running? No Yes
Got a char?
No
New Move ?
doPreMove
Get Command
Trajectory in Progress?
Yes
doMove
doError Computer position and velocity error
No
In list?
Yes
Servo On ? Yes doServo Compute PID Compute PWM value Write PWM value
Next Command
Execute Command Function
No
End of list? Yes Send ERROR message Send response message
CAPFLAG ?
Yes
doCapture Regs For PICMASTER based debug Output position, velocity, etc. info.
Restore Registers
RETURN
DS00532B-page 6
4-190

The following events must occur in the interrupt service routine: Read Timers (RTCC & 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.250mS to execute with 16MHz 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: X 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
Move Status: Y
TABLE 1 - PARAMETERS
Parameter Velocity Limit Acceleration Limit Kp: Proportional Gain Kd: Differential Gain Ki: Integral Gain #h 00 01 02 03 04 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)
DS00532B-page 7
4-191

Read Index position: I
Returns the last index position captured in position counts. Set Position (Value): H, [-8,388,60810 to 8,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 - TYPICAL SERVO RESPONSE: DESIRED/ACTUAL POSITION

2250 2200
Position Response
Position
2150 2100 2050 2000 0 50
Kp = 2048 Kd = 20480 Ki = 1024 Actual Desired
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.
100
150
Time (mSec)
POSITION ERROR
15 10
Position Error
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)
Error
0 -5 -10 -15 -20 -25
20
40
60
80
100
120
140
160
Time (mSec)
Kp = 2048 Kd = 20480 Ki = 1024
M 1000<CR> ;Set velocity to 1000 M-1000<CR> ;Set velocity to 1000 in reverse ;direction
DESIRED/ACTUAL VELOCITY
20 16
Velocity Response
OPTIMIZING THE SYSTEM

Velocity 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 incircuit emulator with unlimited break-point capability, 8K deep trace buffer and external logic probes. Its user interface software runs under Windows 3.1 with pulldown menus and on-line help. The PICMASTER software also support dynamic data exchange (DDE) through which it is 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 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
12 8 4 0 -4 20 40 60 80 100
Kp = 2048 Kd = 20480 Ki = 1024 Actual Desired
120
140
160
Time (mSec)
Velocity = counts/sample
VELOCITY ERROR
4 2
Velocity Error
Error
0 -2 -4 -6 -8 Kp = 2048 Kd = 20480 Ki = 1024 20 40 60 80 100 120 140 160
Time (mSec)
Velocity = counts/sample
DS00532B-page 8
4-192

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. Under windows multitasking environment, using 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. 2. 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. 3. 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. Once these setup 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 PICMASTERs 8K trace buffer.
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 @8MHz Max Encoder Rate Servo Update Time Max Sampling Frequency 1MHz 4 KHz PIC17C42 PIC17C42 @16MHz @25MHz 3.3 MHz 0.25 ms 2-3 KHz 4.5 MHz 0.16 ms 4-5 KHz
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 Windows is a trademark of Microsoft Corporation.
DS00532B-page 9
4-193
A
MAX232 14 Tx1 DO1 11 13 Rx1 DI1 12 +24V TxD RxD +5V 2 +1 Address 2 C8 10E16R IO +5V 1 En OSC GND 1 C4 .1R +5V Address ~RS R2 12KPA 2 27 4 MHz 2-D1 2 7 20 RP1 Vcc 14 2 1 8 OSC1 16 MHz 19 Index CountUp GPO~ LED RxD TxD 26 25 24 23 22 21 +1 C7 10E16R -10V 7 8 2 Tx2 Rx2 +10V C2+ C1- 3 C1+ 1 PIC17C42 30 RE0/ALE 29 RE1/~OE 28 RE2/~WR ALE ~OE~ ~WR 4 +1 C2U2 MAX232 DO2 10 DI2 9 -10V 6 IDrive 3-D1
B C D E F
APPENDIX A: SCHEMATIC DIAGRAM
DS00532B-page 10
1
C5 2 5 10E16R
6 7 4 2 3 8 9 1 5
DSR RTS +10V DTR Tx Rx CTS RI C6 +1 10E16R 2 +5V
P1 !DB9Mini
U3 X2402
2 1 3
RD7/AD15 RD6/AD14 RD5/AD13 RA0/INT RD4/AD12 RA1/RT RD3/AD11 RA2 RD2/AD10 RA3 RD1/AD9 RA4/RX/DT RD0/AD8 RA5/TX/CK RC7/AD7 RC6/AD6 OSC1 RC5/AD5 RC4/AD4 RC3/AD3 OSC2/CLKOUT RC2/AD2 RC1/AD1 RC0/AD0
33 34 35 36 37 38 39 40 9 8 7 6 5 4 3 2
AD15 AD14 AD13 AD12 AD11 AD10 AD9 AD8 AD7 AD6 AD5 AD4 AD3 AD2 AD1 AD0
4-194
5 AD14 AD15 +5V +5V 1 3 2 2 IO +5V 1 1 1 C2 10TR 2 C1 .1R +5V U3 V 8 1 2 C9 .1R U2 +5V V G 16 15 G 4 1 2 C10 .1R 1 2 10 31 R14 12KPA LED Vdd 1 R1 12KPA 2 RB7 RB6 RB5/TCLK3 TEST RB4/TCLK12 RB3/PWM2 32 RB2/PWM1 ~MCLR/Vpp RB1/CAP2 RB0/CAP1 Vss Vss
EEPROM 256 x 8 CS0 TEST 7 CS1 MODE[SPI] SCL[OUT] SCL[IN]
~WR ~OE ALE AD15 AD14 AD13 AD12 AD11 AD10 AD9 AD8 AD7 AD6 AD5 AD4 AD3 AD2 AD1 AD0 18 -Limit~ 17 +Limit~ 16 CountDn 15 14 PWM2 13 PWM1 12 GPI~ 11 CAP1 GPO~ +Limit~ CountDn -Limit~ PWM2 PWM1 GPI~ ExtCAP
SDA
Q1 2N3904
34 33 1 2 5 24 23 22 21 19 17 15 13 11 9 7 20 18 16 14 12 10 8 6 32 31 30 29 28 27 26 25 3 4
R3 360PA
U1 PIC17C42
CAPTURE 6 4 OC ExtCAP 5 Int. Ext. CAP1 P5
P2 HDR17x2-M
LED1 !LN26RP
4
IO 3-A1, 2-E1
PIC17C42 Based DC Servo Control System
Sheet 1 of 3 Controller Logic

B C
Microchip Technology Incorporated
A
+5V 1 1 1 1 2 4X 1X 4 U4 2 CountDn CountUp U4 1 SCALE 10 9 4 MHz 1-C2 2 RP1 RP1 IO 1-F4, 3-A1 1 RP1 2.7Kx9CP 3 1 14 RP2 C13 56PR 2 1 2 5 1 C15 +5V RP2 7 R15 12KPA 2 1 GPO~ HW1 PWB_MCEVM1.0 HW2 socDIP_40.6M 3 2 1 +5V 2 56PR Q2 2N3904 +24V 2 2 +5V Q3 ZTX451 IO 1 PWB 11 +Limit~ 1 2 5 1 2 6 RP2 2.7Kx7ID 1 2 C28 56PR U4 74HC14 9 13 12 C27 56PR GPI~ U4 +5V U4 V G 14 7 1 2 C11 .1R +5V U5 V G 20 10 1 2 C12 .1R RP2 10 -Limit~ 11 10 C26 56PR U4 HW3 JMP.1 9 8 HW4 socDIP_20.3M 1 1 360PA 2 3 R4 U4 6 Index RP2 C14 56PR 12 13 3 P5 HDR3X2-M 3
B C D E F
+5V
RP1 5
RP1 4
4 3 5 2 1

19 18 17 16 15 14 13 12 1 2 3 4 5 6 7 8 9 11 8 QuadDecUD CLK Reset~ CountDn CountUp X4 NC 0 NC 90 90D I5 0D I6 S4 I7 S2 I8 OE~
P3 MOLEX5-M
RP2
GPO
3 2 1
CR1 2 1N4001
APPENDIX A (CONT.): SCHEMATIC DIAGRAM
4-195
1
P9 !WTB3x.1
P8 HDR2-M
+Limit
RP1 RP1 RP1 6 8 7 RP2 4
-Limit
6 5 4 3 2 1
GPI
P4 !WTB6x.1
Sheet 2 of 3 Encoder and I/O
DS00532B-page 11
B

C D E F
4
3
A
+24V +5V 3 5 1 4 Brake 7 P7 !Minifit-4 2 C18 +5V R8 11 1 10NR BS2 Ground 12KPA 2 1 TFlag 9 ISense 8 R9 ISense 2.21K%1PA IDrive 2 1 1 + 2 3 U8 R6 12KPA 1 -10V +24V 1 2 +1 C24 820E50R 2 P6 !Minifit-2 +24V R12 0.47RPB 2 1 L1 120H +5V 1 2 4 GND -Ref 5 CR2 1 1N5819 2 R11 56.2K%1PA 1 +10V U8 V 8 C16 .1R 1 2 G 4 2 C25 1 .1R U6 MC34063 2 1 R10 18.7K%1PA C23 2 680E6.3R +1 2 CR5 2 1N4689 2 1 +5V 2 R7 12KPA 1 2 2 CR4 1 1N4148 CR3 1 1N4148 OC IO 2 C19 .1R Motor+ 1 Motor- 2 3 4 6
B C D E F
IO 1-F4, 2-E1
DS00532B-page 12
U7 LMD18200 LMD18200 1 BS1 Vs 2 C17 Dir 1 10NR Out1 2 PWM Out2 10
PWM1
_ +
5 6
2 C20 68T10R 1
+ _
U8 LF412 7
APPENDIX A (CONT.): SCHEMATIC DIAGRAM
4-196
MC34063 6 Vcc ISense 7 3 Ct 1 2 DriveC 8 SwC 1 SwE 2
+24V
C21 .1R
C22 150PR
4
-10V
Sheet 3 of 3 Power Supply and Output

B C

D E F

APPENDIX B: ENCODER PLD EQUATIONS
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 & +!P0D & !S2 & P90D & +!P0D & S2 & !P90D & + P0D & !S2 & P90D & + P0D & S2 & P90D & +!P0D & !S2 & !P90D & +!P0D & S2 & P90D & + P0D & !S2 & !P90D & ) & !RESET; UP := ( !P0D & S2 & !P90D & +!P0D & S2 & P90D & +!P0D & S2 & P90D & + P0D & S2 & P90D & + P0D & !S2 & P90D & + P0D & !S2 & !P90D & + P0D & !S2 & !P90D & +!P0D & !S2 & !P90D & ) & !RESET; END; END QuadDivider; This design
{ Phi0 } { Phi90 }
S4 !S4 !S4 S4 !S4 S4 S4 !S4
& X4 { { & X4 { & X4 { & X4 { { & X4 { & X4 {
C1 C2 C3 C4 C5 C6 C7 C8
} } } } } } } }
S4 S4 !S4 !S4 !S4 !S4 S4 S4
DS00532B-page 13
4-197

APPENDIX C: (PART 1): PID ALGORITHM FLOWCHART
Enter doServo
Compute Error Value U0 Limit exceeded No Convert YPWM from unipolar to bipolar bipolar to unipolar Set YPWM = 0
Compute proportional Y = Kp.U0
Is Y saturated? No Compute integral of Error U = U + U0 compute integral portion Y = Y + Ki. U
Bypass integral "Anti-wind-up"
LMD18200 requires that PWM duty cycle is not 0% or 100%
YPWM = 0? Compute differential Y = Y + Kd. (U0 - U1) YPWM = 100% Scale Y Y = Y.2 SHIFTNUM Zero lowest 6 bits of YPWM
Yes
Set YPWM 1%
Yes
Set YPWM 99%
Positive overflow in Y No Negative overflow in Y No
Yes
Set Y to max pos value = 007FFFFFh
Write YPWM high byte PW1DCH Write YPWM high byte PW1DCL Yes Set Y to max neg value = FF800000h
Save current error value as previous error U1 = U0
Extract YPWM from YK YPWM = Y <23:8>
Return
Check external position min./max. limit inputs
DS00532B-page 14
4-198

APPENDIX C: (PART 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 SETF CLRF BSF CLRF CLRF Y+B2,WREG 0x7F Y+B3 WREG Y+B3 zero6bits SATFLAG Y+B3 Y+B2 Y+B2,MSB Y+B1 Y+B0 ; check if Y <= -2**23 If negative overflow, saturate y to maximum negative value 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 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 antiwind; not accumulate integrator up ; compute KI*INTEGRAL ; Y=KP*U0+KI*INTEGRAL ; compute KV*(U0-U1) Basic PID calculation
4
; Y=KP*U0+KI*INTEGRAL+KV*(U0-U1) ; scale Y by SHIFTNUM ; Y = Y * (2**SHIFTNUM) Scale Y
If positive overflow, saturate y to maximum positive number
; if not, zero 6 bits ; if so, set Y=0x007FFFFF ; clear for debug purposes
; if not, zero 6 bits ; if so, set Y = 0xFF800000
DS00532B-page 15
4-199

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% 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 unipolar to bipolar EXTSTAT,BIT6 mplimitok YPWM mplimitok Y+B1,YPWM+B0 0xC0 YPWM+B0 YPWM+B1,MSB tmlimit If external position limits have been reached then zero PWM output. ; move Y to YPWM and zero 6 bits ; entry point for torque mode
; correct by 1 LSB ; add one to bit5 of PW1DCL
PWM cycle must not be 0% of 100%
; saturate to max
; saturate to min
; push errors into U(k-1)
DS00532B-page 16
4-200

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 CLRF MOV16 SUB16 ; Now do DownCounter MVFP16 readDown MOVLB MOVPF MOVPF CPFSEQ GOTO MOVPF MVFP16 SUB16 SUB16 CLRF BTFSC tion SETF MVELOCITY+B3 BANK2 TMR3H,WREG TMR3L,DOWNCOUNT+B0 TMR3H readDown WREG,DOWNCOUNT+B1 DOWNCOUNT+B0,TMP+B2 TMP+B0,TMP+B2 TMP+B2,MVELOCITY+B1 MVELOCITY+B3 MVELOCITY+B2,MSB ; timers in Bank 2 DOWNCOUNT,TMP+B0 ; save old downcount RTCCH,WREG RTCCL,UPCOUNT+B0 RTCCH readUp WREG,UPCOUNT+B1 MVELOCITY+B0 UPCOUNT,MVELOCITY+B1 TMP+B0,MVELOCITY+B1 UPCOUNT,TMP+B0 ; save old upcount
; Skip next if HI hasnt changed ; HI changed, re-read LO ; OK to store HI now ; clear bits below binary point ; compute upcount increment
4
3
; Skip next if HI hasnt changed ; HI changed, re-read LO ; OK to store HI now ; compute downcount increment
; compute new measured velocity ; sign extend measured velocity for ; 24 bit addition to measured posi-
ADD24
MVELOCITY+B1,MPOSITION; compute new measured position ; delta position = measured velocity
RETURN ;*****************************************************************************
DS00532B-page 17
4-201

APPENDIX E: IMPLEMENTATION DETAILS OF TRAJECTORY GENERATION
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. 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 A - SPEED PROFILE FOR TRAPEZOIDAL MOVES

T2 x y T2+1
half move
T1
FIGURE B - SPEED PROFILE FOR TRIANGULAR MOVES

T2=T1
FIGURE C - SPEED PROFILE FOR VELOCITY MOVES

final velocity
initial velocity
DS00532B-page 18
4-202

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.
4
3
DS00532B-page 19
4-203

APPENDIX F: COMPLETE CODE LISTING (DCMOTOR.LST)
MPASM B0.54 Revision: 2.0, 27 June 93 PAGE 1
SubTitle
Revision:
2.0, 27 June 93
;************************************************************************* ; ; Revised: 8/5/92 ; CREDIT: Developed by Teknic Inc. 1992 ; ; Assembled using MPASM. Users with ASM17 are suggested to get Microchips ; new universal assembler (MPASM). To assemble with ASM17, all if, else ; endif directives must be replaced by #if, #else and #endif ; respectively. ;************************************************************************* PROCESSOR LIST PIC17C42 COLUMNS=120, XREF=YES, NOWRAP,LINES=255, R=DEC
;************************************************************************* ; 00F4 2400 03E8 07D0 1770 2580 MASTER_CLOCK _SAMPLE_RATE _ENCODER_RATE _RATED_SPEED _BAUDRATE_ set set set set set 16000000 1000 2000 6000 9600 ; Input Clock Freq in Hz ; Sample rate in Hz ; 2000 Pulses/rev ; in RPM
;****************************************************************************
include 17c42.h
include 17c42.mac
; General Purpose Macros
0058 0059 005A 005B
#define #define #define #define
_PICMASTER_DEBUG _SERVO_PID DECIO _SERIAL_IO
TRUE TRUE TRUE TRUE
; Enable PIC-MASTER TRACE Capture ; PID computation based on error ; true for decimal, false for hex
include dcmotor.h17 ; Initialization, Global Defs, ;************************************************************************* ; ; Header file for dcmotor.asm: ; Revised: 8/5/92 ;************************************************************************* ; ; hardware constants ; ; 005C 003D 0900 #define _SET_BAUD_RATE(bps) ((10*MASTER_CLOCK/(64*bps))+5)/10 - 1 CLKOUT set MASTER_CLOCK >> 2 ; Clock Out = CLKIN/4
0006 003F 00FF
TCON1_INIT TCON2_INIT PR1_INIT
set set set
0x06 0x3F 0xFF
; set pwm frequency to CLKOUT/256
DS00532B-page 20
4-204

000F PR2_INIT ;pw1dcH_INIT ;pw1dcL_INIT 007F 00C0 0080 0090 0020 0019 00F3 0000 PW1DCH_INIT PW1DCL_INIT RTCSTA_INIT RCSTA_INIT TXSTA_INIT SPBRG_INIT set set set set set set set set set ((10*MASTER_CLOCK/(4*(PR1_INIT+1)*_SAMPLE_RATE))+5)/ (((PR1_INIT+1) << 8)) >> 9 (((((PR1_INIT+1) << 8)) >> 1) & 0xff) 0x7F 0xC0 0x80 0x90 0x20 _SET_BAUD_RATE(_BAUDRATE_)
0040 0001 0080 00FE
0018 0004 001C 0004
DDRB_INIT set 0xF3 DDRD_INIT set 0x00 ; ; ; max and min pwm values ; PWMINL set 0x40 PWMINH set 0x01 ; 0x0000 + 0x0140 (min 10 bit PWMAXL set 0x80 PWMAXH set 0xFE ; 0xFFC0 - 0x0140 ( max 10 bit ; ; ;************************************************************************* ; global variables ; CBLOCK 0x18 DPX,DPX1,DPX2,DPX3 ; arithmetic accumula AARG,AARG1,BARG,BARG1 ; multiply arguments ENDC CBLOCK 0x18 TMP,TMP1,TMP2,TMP3 MOVTMP,MOVTMP1,MOVTMP2,MOVTMP3
4
3
0018 0004 001C 0004 ENDC CBLOCK 0020 0023 0026 0026 0028 002A 002C 002C 002D 002F 0031 0031 0034 0034 0037 0039 003D 0040 0040 0043 0046 0049 004C 004C 004E 0051 0051 0053 0055 0058 005B 0003 0003 0000 0002 0002 0002 0000 0001 0002 0002 0000 0003 0000 0003 0002 0004 0003 0000 0003 0003 0003 0003 0000 0002 0003 0000 0002 0002 0003 0003 0000
; temporary variables ; move temporary
0x20 VL,VL1,VL2 AL,AL1,AL2 KP,KP1 KV,KV1 KI,KI1 IM FV,FV1 FA,FA1 VALBUF,VALBUF1,VALBUF2 DVALBUF,DVALBUF1,DVALBUF2 ISRBSR,ISRWREG CMDCHAR,CMDTEMP,CMDPTRH,CMDPTRL PARTEMP,PARLEN,PARPTR 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
; velocity limit ; acceleration limit ; proportional gain ; velocity gain ; integral gain ; integrator mode ; velocity feedforward ; acceleration ; iovalue buffer ; ; ; ; ; ; ; ; iovalue buffer isr save storage command interface parameter variables shutter shutter shutter shutter commanded commanded measured measured
; string io variables ; hex io variables
; original commanded ; commanded position ; commanded velocity
DS00532B-page 21
4-205

005B 005F 0063 0067 0069 0069 0x80 006A 006C 006E 0070 0071 0072 0072 0075 0077 0079 007C 007F 007F 0080 0080 0084 0088 0088 008C 0090 0090 0091 0092 0093 0094 0095 0096 0098 0098 009C 009C 00A0 00A4 00A8 00AC 00AC 00AE 00B0 00B0 00B4 00B6 00B9 00B9 00BA 00BA 00BB 00BB 00BB 00BB 00BC 00BE 00C0 00C0 0004 0004 0004 0002 0000 0001 for 0002 0002 0002 0001 0001 0000 0003 0002 0002 0003 0003 0000 0001 0000 0004 0004 0000 0004 0004 0000 0001 0001 0001 0001 0001 0001 0002 0000 0004 0000 0004 0004 0004 0004 0000 0002 0002 0000 0004 0002 0003 0000 0001 0000 0001 0000 0000 0000 0001 0002 0002 0000 0000 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 U0,U01,U1,U11 YPWM,YPWM1,YPWM2,YPWM3 YPWMIN,YPWMIN1,YPWMAX,YPWMAX1 SERVOFLAG MODETYPE EXTSTAT MOVSTAT MOVFLAG SATFLAG INTEGRAL,INTEGRAL1 DECVAL,DECSTAT,DECTMP,DECSIGN 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 ; ; ; ; move move half move value value move value time in sample
; 0x00 for positive, ; ; ; ; ; time to maximum time for half the total move time next move modetype move modetype
; measured position ; measured velocity ; position error ; velocity error ; multiply sign ; Y(k) before pwm ; saturated error at ; pwm input ; pwm input limits ; ; ; ; ; ; ; servoflag = 0 => no mode external status move status register move flag saturation flag integrator
; decimal io variables ; ; ; ; commanded commanded commanded commanded accelera velocity = position velocity
; running up counter ; running down counter ; move discretization ; phase 2 flat itera ; position at last ; # of bit shifts from
if _PICMASTER_DEBUG ;************************************************************************** ; For PICMASTER Debug/servo tuning Purposes Only CAPFLAG CAPCOUNT,CAPCOUNT1 CAPTMP,CAPTMP1 ; trace capture flag ; PICMASTER trace ; trace capture
;**************************************************************************
DS00532B-page 22
4-206

00C0 00C1 00C1 00C3 00C5 00C7 00C8 0001 0000 0002 0002 0002 0001 0000 endif ZERO,ONE tblptrlTemp,tblptrhTemp TblLatLo, TblLatHi alustaTemp ENDC ;************************************************************************* ; 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 BCF BCF BCF BCF ENDM ;************************************************************************* _fs0 _fs1 _fs2 _fs3 _fs0 _fs1 _fs2 _fs3 _fs0 _fs1 _fs2 _fs3 ; ; ; ; constants temp TABLE Pointers for Table Latch for ISR save temp alusta
4
3
;*************************************************************************
DS00532B-page 23
4-207

; ; ; ; ; ; ; ; NAME: DESCRIPTION: ARGUMENTS: LOADAB Loads extended math library AARG and BARG A => AARG B => BARG
TIMING (cycles): 4 A,B MOVFP MOVFP MOVFP MOVFP A+B0,AARG+B0 A+B1,AARG+B1 B+B0,BARG+B0 B+B1,BARG+B1 ; ; ; ; 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
LOADAB MACRO
ENDM ;************************************************************************** ;************************************************************************** ; ascii constants ; CR set 0x0D CAN set 0x18 BS set 0x08 SP set 0x20 LF set 0x0A MN set - ; ;********************************************************* ; 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 DO_MODE set O ; O DO_SETPARAMETER set S ; S DO_READPARAMETER set R ; R DO_SHUTTER set C ; C DO_READCOMPOSITION set P ; P DO_READCOMVELOCITY set V ; V
000D 0018 0008 0020 000A 002D
0001
0008
0021 003F
0000 0001 0002 0003 0000 0001 0002 0003
000D 004D 004F 0053 0052 0043 0050 0056
DS00532B-page 24
4-208

0070 0076 0058 0059 0049 0048 005A 0073 0063 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 p v X Y I H Z s c ; ; ; ; ; ; ; ; ; 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 FUNC,ROOT DATA DATA ENDM 0002 CMD_ENTRY_LENGTH set 2 ROOT FUNC
4
3
;************************************************************************* ;************************************************************************* ; NAME: CMD_START ; ; DESCRIPTION: Labels the start of the command table. ; 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 ;
;*************************************************************************
; ; ;************************************************************************** ; PID Constantnts ; define PIV parameters, computation based on errors only. Does not involve
DS00532B-page 25
4-209

; ; No Load @ 2Khz : Kp=3600, Ki=112, Kd= 28800, Shiftcount = 3 ; With Indicator Load @ 2Khz, Kp=2300 Ki= 41, Kd= 32200, Shiftcount = 4 ; @ 4Khz Kp=1024, Ki=8, Kd=31405 , ShiftCount = 5 ; @ 0.5 Khz Kp=5400, Ki=310, Kd=23400 , ShiftCount = 2 ; ; No Load @ 1Khz Kp=3600, Ki=192, Kd=16800, ShiftNum = 3 ; No Load @ 1Khz Kp=1800, Ki=52, Kd=15600, ShiftNum = 4 ; ; Adjust Shiftcount by maximizing Kd (for a 16 bit signed num) ; ;**************************************************************************** 005D 005E 005F 3200 2000 #define _KP #define _KI #define _KV _VEL_LIMIT _ACCL_LIMIT 1800 52 15600 set set ; 16 bit Kp ; 16 bit Ki ; 16 bit Kv ((_RATED_SPEED*_ENCODER_RATE)/; 1/4 Rated sp 0x2000 ; use smaller
0004
if _SERVO_PID == TRUE _SHIFTNUM endif
equ
;*************************************************************************
0000 C021
ORG goto ORG goto
0x0 Startup 0x20 InterruptPoll
; reset vector ; startup
0020 C07D
; interrupt
;************************************************************************* ; 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. ; ; RETURNS: restart to safe and initial state ; ; STACK UTILIZATION: none ; TIMING (in cycles): X Startup 0021 8406 bsf _glintd AUTONO ; no auto 0022 0023 0024 0025 8404 8504 8604 8704 BSF BSF BSF BSF _fs0 _fs1 _fs2 _fs3 ; disable all
0026 B018 0027 4A01 memloop 0028 2900 0029 1F01 002A C028 002B 15C2
movlw movpf
0x18 wreg,fsr0
; clear all
clrf incfsz goto incf
indf0 fsr0 memloop ONE
DS00532B-page 26
4-210

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 003B 750A movlb movlw movfp movlw movfp movfp movlw movfp movfp movlw movfp movlb movlw movfp movlw movfp 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 wreg,pr2 ; set organization of timers ; bank3 ini
; set duty
; bank2 initialization
; initialize timer1 period
; initialize timer2 period
003C B800 003D B080 003E 650A 003F B0F8 0040 0110
movlb movlw movfp movlw movwf if _SERIAL_IO
bank0 RTCSTA_INIT wreg,rtcsta 0xF8 porta
; bank0 initialization
4
3
; sets RTC for external input ; RA2 connected to BREAK Input of LMD18200 ; On Reset, thus pulled high breaking the motor
0041 B090 0042 730A 0043 B020 0044 750A 0045 B019 0046 770A endif 0047 B0F3 0048 710A 0049 B801
movlw movfp movlw movfp movlw movfp
RCSTA_INIT wreg,rcsta TXSTA_INIT wreg,txsta SPBRG_INIT wreg,spbrg
; set receive status ; set transmit status
; set baud rate
movlw movfp movlb
DDRB_INIT wreg,ddrb bank1
; set port B for whatever ; bank1 initialization
if (_SERVO_PID == TRUE) MOVK16 _KP,KP 004A 004B 004C 004D B008 0126 B007 0127 MOVLW MOVWF MOVLW MOVWF (1800) & 0xff KP+B0 ((1800) >> 8) KP+B1
MOVK16 004E 004F 0050 0051 B034 012A B000 012B MOVLW MOVWF MOVLW MOVWF
_KI,KI
(52) & 0xff KI+B0 ((52) >> 8) KI+B1
MOVK16
_KV,KV
DS00532B-page 27
4-211

0052 0053 0054 0055 B0F0 0128 B03C 0129 MOVLW MOVWF MOVLW MOVWF (15600) & 0xff KV+B0 ((15600) >> 8) KV+B1
endif MOVK16 0056 0057 0058 0059 B000 0123 B020 0124 MOVLW MOVWF MOVLW MOVWF _ACCL_LIMIT,AL
(_ACCL_LIMIT) & 0xff AL+B0 ((_ACCL_LIMIT) >> 8) AL+B1
MOVK16 005A 005B 005C 005D B000 0120 B032 0121 MOVLW MOVWF MOVLW MOVWF
_VEL_LIMIT,VL
(_VEL_LIMIT) & 0xff VL+B0 ((_VEL_LIMIT) >> 8) VL+B1
005E B004 005F 01B9 0060 5289 0061 0062 0063 0064 0065 0066 0067 0068 0069 006A 006B 006C B080 4A8E B0FE 4A8F B040 4A8C B001 4A8D 2916 2907 8517 8307
movlw movwf movpf movlw movpf movlw movpf movlw movpf movlw movpf clrf clrf bsf bsf bcf movlb zeroctrs
_SHIFTNUM SHIFTNUM pw1dch,YPWM+B1 PWMAXL ; initialize pwm limits wreg,YPWMAX+B0 PWMAXH wreg,YPWMAX+B1 PWMINL wreg,YPWMIN+B0 PWMINH wreg,YPWMIN+B1 pir intsta _tm2ie _peie _glintd bank2 ; clear flags, set indiviual interrupts
006D 8C06 006E B802
; enable interrupts
006F 290B 0070 290C 0071 2912 0072 2913 0073 B0FF 0074 170A 0075 C074 0076 0077 0078 0079 007A 007B 6A0B 080C 0812 0813 330A C06F
clrf clrf clrf clrf movlw decfsz goto movfp iorwf iorwf iorwf tstfsz goto goto
rtccl rtcch tmr3l tmr3h 0xFF wreg delay rtccl,wreg rtcch,W tmr3l,W tmr3h,W wreg zeroctrs PollingLoop
; clear up counter
; clear down counter
delay
; motor still moving
007C C124
;************************************************************************
DS00532B-page 28
4-212

;************************************************************************* ; NAME: InterruptPoll
InterruptPoll
007D 007E 007F 0080 0081 0082 0083 0084
0138 6A04 01C7 4F37 4DC3 4EC4 A0C5 A2C6
movwf movfp movwf movpf movpf movpf tlrd tlrd
ISRWREG alusta,wreg alustaTemp bsr,ISRBSR tblptrl,tblptrlTemp tblptrh,tblptrhTemp 0,TblLatLo 1,TblLatHi
; save W Reg ; save alusta ; save BSR,wreg ; save Table Pointers
; save Table Latch
0085 B801 0086 E0E5
movlb call
bank1 doMPosMVel ; calculate measured position and ; velocity ; evaluate external status ; ; ; ; if MOVFLAG=0 and MOVSTAT,bit7=1 then do premove. This is only executed once at the beginning of each move
0087 E111 0088 0089 008A 008B 008C 2293 B501 0494 9F0A E23D
call rlncf andlw subwf btfsc call btfsc call call tstfsz call if
doExtstat MOVSTAT,W 0x01 MOVFLAG,W wreg,MSB doPreMove MOVSTAT,bit6 doMove doError SERVOFLAG doServo
4
3
008D 9E93 008E E30F 008F E09E 0090 3390 0091 E4AC
; is motion continuing? ; if so, do move ; calculate position and velocity ; error ; test servoflag, if 0 then no servo ; do servo
0092 33BB 0093 E79A endif 0094 B801 0095 2916 0096 0097 0098 0099 009A 009B 009C A4C5 A6C6 6DC3 6EC4 6F37 64C7 6A38
_PICMASTER_DEBUG tstfsz CAPFLAG call doCaptureRegs
; for PIC-MASTER Trace Capture, demo
movlb clrf tlwt tlwt movfp movfp movfp movfp movfp retfie
bank1 pir 0,TblLatLo 1,TblLatHi tblptrlTemp,tblptrl tblptrhTemp,tblptrh ISRBSR,bsr alustaTemp,alusta ISRWREG,wreg ; clear all interrupt request flags
; restored table latch ; restored table pointers ; restore BSR,wreg,alusta & Table
009D 0005
;************************************************************************* ;************************************************************************* ; NAME: doError ; ; DESCRIPTION: Calculates the position and velocity error. ; doError MOV24 POSITION,POSERROR ; calculate position error
DS00532B-page 29
4-213

009E 009F 00A0 00A1 00A2 00A3 6A55 4A79 6A56 4A7A 6A57 4A7B MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF POSITION+B0,wreg wreg,POSERROR+B0 POSITION+B1,wreg wreg,POSERROR+B1 POSITION+B2,wreg wreg,POSERROR+B2 ; ; ; ; ; ; get byte of POSITION into w move to POSERROR(B0) get byte of POSITION into w move to POSERROR(B1) get byte of POSITION into w move to POSERROR(B2)
SUB24 00A4 00A5 00A6 00A7 00A8 00A9 6A72 0579 6A73 037A 6A74 037B MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB
MPOSITION,POSERROR MPOSITION+B0,wreg POSERROR+B0 MPOSITION+B1,wreg POSERROR+B1 MPOSITION+B2,wreg POSERROR+B2 ; ; ; ; ; ; get sub get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of MPOSITION into of POSERROR, MPOSITION into POSERROR, save MPOSITION into POSERROR, save
00AA 9F7B 00AB C0B7 ppos 00AC 00AD 00AE 00AF 00B0 00B1 00B2 00B3 00B4 00B5 00B6 00B7 00B8 00B9 00BA 00BB 00BC 00BD 00BE 00BF 00C0 6A7A B580 097B 290A 327B C0C1 297B B07F 4A7A 2B79 C0C1 pneg 6A7A B37F 0B7B 2B0A 307B C0C1 2B7B 297A 877A 2979 psatok
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 wreg POSERROR+B2 psatok POSERROR+B2 0x7F wreg,POSERROR+B1 POSERROR psatok POSERROR+B1,wreg 0x7F POSERROR+B2 wreg POSERROR+B2 psatok POSERROR+B2 POSERROR+B1 POSERROR+B1,MSB POSERROR
; saturate error to lowest 16 bits
; clear high byte for debug purposes
; set high byte to 0xFF for debug
MOV24 00C1 00C2 00C3 00C4 00C5 00C6 6A58 4A7C 6A59 4A7D 6A5A 4A7E MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF
VELOCITY,VELERROR VELOCITY+B0,wreg wreg,VELERROR+B0 VELOCITY+B1,wreg wreg,VELERROR+B1 VELOCITY+B2,wreg wreg,VELERROR+B2
; calculate velocity error ; ; ; ; ; ; get byte of VELOCITY into w move to VELERROR(B0) get byte of VELOCITY into w move to VELERROR(B1) get byte of VELOCITY into w move to VELERROR(B2)
SUB24 00C7 00C8 00C9 00CA 00CB 00CC 6A75 057C 6A76 037D 6A77 037E MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB
MVELOCITY,VELERROR MVELOCITY+B0,wreg VELERROR+B0 MVELOCITY+B1,wreg VELERROR+B1 MVELOCITY+B2,wreg VELERROR+B2 ; ; ; ; ; ; get sub get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of MVELOCITY into w of VELERROR, save MVELOCITY into w VELERROR, save in MVELOCITY into w VELERROR, save in
00CD 9F7E 00CE C0DA vpos
btfsc goto
VELERROR+B2,MSB vneg
; saturate error to lowest 16 bits
DS00532B-page 30
4-214

00CF 00D0 00D1 00D2 00D3 00D4 00D5 00D6 00D7 00D8 00D9 00DA 00DB 00DC 00DD 00DE 00DF 00E0 00E1 00E2 00E3 6A7D B580 097E 290A 327E C0E4 297E B07F 4A7D 2B7C C0E4 vneg 6A7D B37F 0B7E 2B0A 307E C0E4 2B7E 297D 877D 297C vsatok 00E4 0002 return ;************************************************************************* movfp iorlw andwf setf cpfslt goto setf clrf bsf clrf VELERROR+B1,wreg 0x7F VELERROR+B2 wreg VELERROR+B2 vsatok VELERROR+B2 VELERROR+B1 VELERROR+B1,MSB VELERROR movfp andlw iorwf clrf cpfsgt goto clrf movlw movpf setf goto VELERROR+B1,wreg 0x80 VELERROR+B2 wreg VELERROR+B2 vsatok VELERROR+B2 0x7F wreg,VELERROR+B1 VELERROR vsatok
4
3
;************************************************************************* ; NAME: doMPosMVel ; ; DESCRIPTION: Calculates current position from UpCount and DownCount ; doMPosMVel ; Do UpCounter first MOVFP16 UPCOUNT,TMP+B0 00E5 78AC 00E6 79AD MOVFP MOVFP UPCOUNT+B0,TMP+B0+B0 UPCOUNT+B1,TMP+B0+B1 ; save old upcount ; move UPCOUNT(B0) to TMP+B0(B0) ; move UPCOUNT(B1) to TMP+B0(B1)
readUp 00E7 00E8 00E9 00EA 00EB 4C0A 4BAC 310C C0E7 4AAD movpf movpf cpfseq goto movpf clrf MOV16 00ED 00EE 00EF 00F0 6AAC 0176 6AAD 0177 MOVFP MOVWF MOVFP MOVWF rtcch,wreg rtccl,UPCOUNT+B0 rtcch readUp wreg,UPCOUNT+B1 MVELOCITY+B0 UPCOUNT,MVELOCITY+B1 UPCOUNT+B0,wreg MVELOCITY+B1+B0 UPCOUNT+B1,wreg MVELOCITY+B1+B1
; Skip next if HI hasnt changed ; HI changed, re-read LO ; OK to store HI now ; clear bits below binary point ; compute upcount increment ; ; ; ; get byte of UPCOUNT into w move to MVELOCITY+B1(B0) get byte of UPCOUNT into w move to MVELOCITY+B1(B1)
00EC 2975
SUB16 00F1 00F2 00F3 00F4 6A18 0576 6A19 0377 MOVFP SUBWF MOVFP SUBWFB
TMP+B0,MVELOCITY+B1 TMP+B0+B0,wreg MVELOCITY+B1+B0 TMP+B0+B1,wreg MVELOCITY+B1+B1 ; ; ; ; get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of of TMP+B0 into of TMP+B0 into w MVELOCITY+B1,
DS00532B-page 31
4-215

; Now do DownCounter MOVFP16 00F5 78AE 00F6 79AF MOVFP MOVFP DOWNCOUNT,TMP+B0 DOWNCOUNT+B0,TMP+B0+B0 DOWNCOUNT+B1,TMP+B0+B1 ; save old downcount ; move DOWNCOUNT(B0) to ; move DOWNCOUNT(B1) to
readDown 00F7 00F8 00F9 00FA 00FB 00FC B802 530A 52AE 3113 C0F7 4AAF movlb movpf movpf cpfseq goto movpf MOVFP16 00FD 7AAE 00FE 7BAF MOVFP MOVFP bank2 ; timers in Bank 2 tmr3h,wreg tmr3l,DOWNCOUNT+B0 tmr3h ; Skip next if HI hasnt changed readDown ; HI changed, re-read LO wreg,DOWNCOUNT+B1 ; OK to store HI now DOWNCOUNT+B0,TMP+B2 DOWNCOUNT+B0+B0,TMP+B2+B0 DOWNCOUNT+B0+B1,TMP+B2+B1 ; compute downcount increment ; move DOWNCOUNT+B0(B0) to ; move DOWNCOUNT+B0(B1) to
SUB16 00FF 0100 0101 0102 6A18 051A 6A19 031B MOVFP SUBWF MOVFP SUBWFB
TMP+B0,TMP+B2 TMP+B0+B0,wreg TMP+B2+B0 TMP+B0+B1,wreg TMP+B2+B1 ; ; ; ; get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of of TMP+B0 into of TMP+B2, save TMP+B0 into w TMP+B2, save in
SUB16 0103 0104 0105 0106 6A1A 0576 6A1B 0377 MOVFP SUBWF MOVFP SUBWFB
TMP+B2,MVELOCITY+B1 TMP+B2+B0,wreg MVELOCITY+B1+B0 TMP+B2+B1,wreg MVELOCITY+B1+B1
; compute new measured velocity ; ; ; ; get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of of TMP+B2 into of TMP+B2 into w MVELOCITY+B1,
0107 2978 0108 9F77 0109 2B78
clrf btfsc setf
MVELOCITY+B3 MVELOCITY+B2,MSB MVELOCITY+B3
; sign extend measured velocity ; 24 bit addition to measured
ADD24 010A 010B 010C 010D 010E 010F 6A76 0F72 6A77 1173 6A78 1174 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC
MVELOCITY+B1,MPOSITION MVELOCITY+B1+B0,wreg MPOSITION+B0 MVELOCITY+B1+B1,wreg MPOSITION+B1 MVELOCITY+B1+B2,wreg MPOSITION+B2
; 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 MVELOCITY+B1 of MPOSITION, MVELOCITY+B1 MPOSITION, save MVELOCITY+B1 MPOSITION, save
; delta position = measured 0110 0002 return ;************************************************************************* ; NAME: doExtstat ; ; DESCRIPTION: Get +limit,-limit,GPI from PORTB and set in EXTSTAT ; doExtstat btfss _intir goto otherbits MOV24 MPOSITION,INDEXPOS
0111 9407 0112 C11B
DS00532B-page 32
4-216

0113 0114 0115 0116 0117 0118 6A72 4AB6 6A73 4AB7 6A74 4AB8 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MPOSITION+B0,wreg wreg,INDEXPOS+B0 MPOSITION+B1,wreg wreg,INDEXPOS+B1 MPOSITION+B2,wreg wreg,INDEXPOS+B2 ; ; ; ; ; ; get byte of MPOSITION into w move to INDEXPOS(B0) get byte of MPOSITION into w move to INDEXPOS(B1) get byte of MPOSITION into w move to INDEXPOS(B2)
0119 8C07 011A 8792
bcf bsf otherbits movlb movfp rrcf andlw movpf swapf iorwf iorwf return
_intir EXTSTAT,MSB
011B 011C 011D 011E 011F 0120 0121 0122
B800 6A12 190A B561 4A18 1D18 0818 0992
bank0 portb,wreg wreg 0x61 wreg,TMP TMP TMP,W EXTSTAT
; get +limit,-limit and GPI ; arrange in correct bit posi
; set in EXTSTAT
0123 0002
;************************************************************************* ;************************************************************************* ; NAME: PollingLoop ; ; DESCRIPTION: The actual polling loop called after the boards ; initialization ; ; ENTRY CONDITIONS: System globals and hardware initialized and the ; interrupt processes started. ; PollingLoop if _SERIAL_IO call call cpfseq goto call movpf goto else clrwdt goto endif ;************************************************************************* include mult.asm ; Double Precision Multiplication Routine ;************************************************************************* ; Double Precision Multiplication Routine ; ; 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 PollingLoop ; wait for Interrupt
4
3
0124 0125 0126 0127
E557 E681 31C2 C124
IdleFunction GetChk ONE PollingLoop GetChar wreg,CMDCHAR DoCommand
; GetChk, is receive buffer full?
0128 E676 0129 4A39 012A C559
; if so, get character ; put in CMDCHAR
DS00532B-page 33
4-217

; 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 ; ;************************************************************************* 0001 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 i = 0 if SIGNED while i < 15 else while i < 16 endif if i < 8 btfsc BARG+B0,i ; test low byte else btfsc BARG+B1,i-8 ; test high byte endif goto add#v(i) if i < 8 rlcf DPX+B3,W ; rotate sign into carry bit rrcf DPX+B3 ; for i < 8, no meaningful rrcf DPX+B2 ; are in DPX+B0 rrcf DPX+B1 else rlcf DPX+B3,W ; rotate sign into carry bit rrcf DPX+B3 rrcf DPX+B2 rrcf DPX+B1 rrcf DPX+B0 endif i = i+1 endw clrf DPX+B0 ; if we get here, BARG = 0 return add0 movfp AARG+B0,wreg addwf DPX+B2 ;add lsb movfp AARG+B1,wreg addwfc DPX+B3 ;add msb rlcf AARG+B1,W ; rotate sign into carry bit rrcf DPX+B3 ; for i < 8, no meaningful rrcf DPX+B2 ; are in DPX+B0 rrcf DPX+B1 i = 1 if SIGNED while i < 15 else while i < 16 endif if i < 8 btfss BARG+B0,i ; test low byte else btfss BARG+B1,i-8 ; test high byte endif goto noadd#v(i) equ TRUE
DS00532B-page 34
4-218

add#v(i) movfp AARG+B0,wreg addwf DPX+B2 movfp AARG+B1,wreg addwfc DPX+B3 noadd#v(i) if i < 8 rlcf AARG+B1,W rrcf DPX+B3 rrcf DPX+B2 rrcf DPX+B1 else rlcf AARG+B1,W rrcf DPX+B3 rrcf DPX+B2 rrcf DPX+B1 rrcf DPX+B0 endf i = i+1 endw if SIGNED rlcf AARG+B1,W rrcf DPX+B3 rrcf DPX+B2 rrcf DPX+B1 rrcf DPX+B0 endif ENDM
;add lsb ;add msb
; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0
; rotate sign into carry bit
; since BARG is always made ; the last bit is known to be
4
3
; Double Precision Multiply ( ; ( AARG*BARG -> : 32 bit ; Dmult if 012B 971F 012C C137 SIGNED btfss BARG+B1,MSB goto argsok NEG16 COMF COMF CLRF INCF ADDWFC COMF COMF CLRF INCF ADDWFC endif argsok 0137 291B 0138 291A MULTMAC 0000 0000 variable i i = 0 if SIGNED while i < 15 else while i < 16 endif if i < 8 btfsc BARG+B0,i else btfsc BARG+B1,i-8 endif goto add#v(i) if i < 8 rlcf DPX+B3,W rrcf DPX+B3 clrf clrf DPX+B3 DPX+B2 ; clear initial partial product ; use macro for multiplication AARG+B0+B0 AARG+B0+B1 wreg AARG+B0+B0 AARG+B0+B1 NEG16 BARG+B0+B0 BARG+B0+B1 wreg BARG+B0+B0 BARG+B0+B1 ; test sign of BARG ; if positive, ok ; if negative, then negate
AARG+B0
012D 012E 012F 0130 0131 0132 0133 0134 0135 0136
131C 131D 290A 151C 111D 131E 131F 290A 151E 111F
BARG+B0
; AARG and BARG
; test low byte ; test high byte
; rotate sign into carry bit ; for i < 8, no meaningful bits
DS00532B-page 35
4-219

rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 endw if i < 8 btfsc else btfsc endif goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfsc else btfsc endif goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif 0002 0145 9A1E i = i+1 if i < 8 btfsc else btfsc endif 0146 C1B0 0147 0148 0149 014A 1A1B 191B 191A 1919 goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf add2 DPX+B3,W DPX+B3 DPX+B2 DPX+B1 DPX+B3,W DPX+B3 DPX+B2 DPX+B1 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 DPX+B2 DPX+B1 DPX+B3,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; are in DPX+B0
0139 981E
BARG+B0,i BARG+B1,i-8 add0 DPX+B3,W DPX+B3 DPX+B2 DPX+B1 DPX+B3,W DPX+B3 DPX+B2 DPX+B1 DPX+B0
013A C19C 013B 013C 013D 013E 1A1B 191B 191A 1919
0001 013F 991E
0140 C1A6 0141 0142 0143 0144 1A1B 191B 191A 1919
BARG+B0,i BARG+B1,i-8
DS00532B-page 36
4-220

rrcf endif i = i+1 if i < 8 btfsc else btfsc endif goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfsc else btfsc endif goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfsc else btfsc endif goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfsc else btfsc endif goto if i < 8 rlcf rrcf DPX+B0
0003 014B 9B1E
014C C1BA 014D 014E 014F 0150 1A1B 191B 191A 1919
0004 0151 9C1E
4
3
0152 C1C4 0153 0154 0155 0156 1A1B 191B 191A 1919
0005 0157 9D1E
0158 C1CE 0159 015A 015B 015C 1A1B 191B 191A 1919
0006 015D 9E1E
BARG+B0,i BARG+B1,i-8 add6 DPX+B3,W DPX+B3
015E C1D8 015F 1A1B 0160 191B
; rotate sign into carry bit ; for i < 8, no meaningful
DS00532B-page 37
4-221

0161 191A 0162 1919 rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfsc else btfsc endif goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfsc else btfsc endif goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfsc else btfsc endif goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfsc DPX+B2 DPX+B1 DPX+B3,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; are in DPX+B0
0007 0163 9F1E
0164 C1E2 0165 0166 0167 0168 1A1B 191B 191A 1919
0008
0169 981F 016A C1EC
016B 016C 016D 016E 016F 0009
1A1B 191B 191A 1919 1918
0170 991F 0171 C1F7
0172 0173 0174 0175 0176 000A
1A1B 191B 191A 1919 1918
BARG+B0,i
; test low byte
DS00532B-page 38
4-222

0177 9A1F 0178 C202 else btfsc endif goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfsc else btfsc endif goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfsc else btfsc endif goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfsc else btfsc endif goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf BARG+B1,i-8 add10 DPX+B3,W DPX+B3 DPX+B2 DPX+B1 DPX+B3,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 ; test high byte
0179 017A 017B 017C 017D 000B
1A1B 191B 191A 1919 1918
017E 9B1F 017F C20D
4
3
0180 0181 0182 0183 0184 000C
1A1B 191B 191A 1919 1918
0185 9C1F 0186 C218
0187 0188 0189 018A 018B 000D
1A1B 191B 191A 1919 1918
BARG+B0,i BARG+B1,i-8 add13 DPX+B3,W DPX+B3 DPX+B2 DPX+B1 DPX+B3,W DPX+B3
018C 9D1F 018D C223
018E 1A1B 018F 191B
DS00532B-page 39
4-223

0190 191A 0191 1919 0192 1918 000E rrcf rrcf rrcf endif i = i+1 if i < 8 btfsc else btfsc endif goto if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 clrf return add0 019C 019D 019E 019F 01A0 01A1 01A2 01A3 0001 6A1C 0F1A 6A1D 111B 1A1D 191B 191A 1919 movfp addwf movfp addwfc rlcf rrcf rrcf rrcf AARG+B0,wreg DPX+B2 AARG+B1,wreg DPX+B3 ;add msb AARG+B1,W DPX+B3 DPX+B2 DPX+B1 ;add lsb DPX+B2 DPX+B1 DPX+B0
0193 9E1F 0194 C22E
0195 0196 0197 0198 0199
1A1B 191B 191A 1919 1918
000F 019A 2918 019B 0002
DPX+B0
; if we get here, BARG = 0
i = 1 if SIGNED while i < 15 else while i < 16 endif if i < 8 btfss BARG+B0,i ; test low byte else btfss BARG+B1,i-8 ; test high byte endif goto noadd#v(i) add#v(i) movfp addwf movfp addwfc noadd#v(i) if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 endw if i < 8 btfss AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 AARG+B0,wreg DPX+B2 ;add lsb AARG+B1,wreg DPX+B3 ;add msb
01A4 911E
BARG+B0,i
; test low byte
DS00532B-page 40
4-224

else btfss endif goto add1 01A6 01A7 01A8 01A9 6A1C 0F1A 6A1D 111B noadd1 01AA 01AB 01AC 01AD 1A1D 191B 191A 1919 if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfss else btfss endif goto movfp addwf movfp addwfc noadd2 01B4 01B5 01B6 01B7 1A1D 191B 191A 1919 if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfss else btfss endif goto add3 01BA 01BB 01BC 01BD 6A1C 0F1A 6A1D 111B noadd3 01BE 01BF 01C0 01C1 1A1D 191B 191A 1919 if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 movfp addwf movfp addwfc AARG+B0,wreg DPX+B2 AARG+B1,wreg DPX+B3 ;add lsb ;add msb AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful bits ; are in DPX+B0 movfp addwf movfp addwfc AARG+B0,wreg DPX+B2 AARG+B1,wreg DPX+B3 ;add lsb ;add msb BARG+B1,i-8 noadd1 ; test high byte
01A5 C1AA
0002 01AE 921E
BARG+B0,i BARG+B1,i-8 noadd2 AARG+B0,wreg DPX+B2 AARG+B1,wreg DPX+B3
; test low byte ; test high byte add2 ;add lsb ;add msb
4
3
01AF 01B0 01B1 01B2 01B3
C1B4 6A1C 0F1A 6A1D 111B
0003 01B8 931E
BARG+B0,i BARG+B1,i-8 noadd3
01B9 C1BE
DS00532B-page 41
4-225

endif 0004 01C2 941E i = i+1 if i < 8 btfss else btfss endif goto add4 01C4 01C5 01C6 01C7 6A1C 0F1A 6A1D 111B noadd4 01C8 01C9 01CA 01CB 1A1D 191B 191A 1919 if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfss else btfss endif goto add5 01CE 01CF 01D0 01D1 6A1C 0F1A 6A1D 111B noadd5 01D2 01D3 01D4 01D5 1A1D 191B 191A 1919 if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 movfp addwf movfp addwfc AARG+B0,wreg DPX+B2 ;add lsb AARG+B1,wreg DPX+B3 ;add msb AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 movfp addwf movfp addwfc AARG+B0,wreg DPX+B2 AARG+B1,wreg DPX+B3 ;add lsb ;add msb
01C3 C1C8
0005 01CC 951E
01CD C1D2
endif
0006
i = i+1 if i < 8 btfss else btfss endif
01D6 961E
BARG+B0,i BARG+B1,i-8
01D7 C1DC add6 01D8 6A1C
goto movfp
noadd6 AARG+B0,wreg
DS00532B-page 42
4-226

01D9 0F1A 01DA 6A1D 01DB 111B noadd6 01DC 01DD 01DE 01DF 1A1D 191B 191A 1919 if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfss else btfss endif goto add7 01E2 01E3 01E4 01E5 6A1C 0F1A 6A1D 111B noadd7 if i < 8 01E6 01E7 01E8 01E9 1A1D 191B 191A 1919 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfss else btfss endif goto add8 01EC 01ED 01EE 01EF 6A1C 0F1A 6A1D 111B noadd8 if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 movfp addwf movfp addwfc AARG+B0,wreg DPX+B2 ;add lsb AARG+B1,wreg DPX+B3 ;add msb AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 movfp addwf movfp addwfc AARG+B0,wreg DPX+B2 ;add lsb AARG+B1,wreg DPX+B3 ;add msb AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 addwf movfp addwfc DPX+B2 ;add lsb AARG+B1,wreg DPX+B3 ;add msb
0007 01E0 971E
01E1 C1E6
4
3
0008
01EA 901F 01EB C1F0
01F0 01F1 01F2 01F3 01F4 0009
1A1D 191B 191A 1919 1918
DS00532B-page 43
4-227

if i < 8 btfss else btfss endif goto add9 01F7 01F8 01F9 01FA 6A1C 0F1A 6A1D 111B noadd9 if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfss else btfss endif goto add10 0202 0203 0204 0205 6A1C 0F1A 6A1D 111B noadd10 if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfss else btfss endif goto add11 020D 020E 020F 0210 6A1C 0F1A 6A1D 111B noadd11 if i < 8 rlcf rrcf rrcf rrcf else AARG+B1,W DPX+B3 DPX+B2 DPX+B1 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 movfp addwf movfp addwfc AARG+B0,wreg DPX+B2 ;add lsb AARG+B1,wreg DPX+B3 ;add msb AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 movfp addwf movfp addwfc AARG+B0,wreg DPX+B2 ;add lsb AARG+B1,wreg DPX+B3 ;add msb AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 movfp addwf movfp addwfc AARG+B0,wreg DPX+B2 ;add lsb AARG+B1,wreg DPX+B3 ;add msb BARG+B0,i BARG+B1,i-8 noadd9 ; test low byte ; test high byte
01F5 911F 01F6 C1FB
01FB 01FC 01FD 01FE 01FF 000A
1A1D 191B 191A 1919 1918
0200 921F 0201 C206
0206 0207 0208 0209 020A 000B
1A1D 191B 191A 1919 1918
020B 931F 020C C211
DS00532B-page 44
4-228

0211 0212 0213 0214 0215 000C 1A1D 191B 191A 1919 1918 rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfss else btfss endif goto add12 0218 0219 021A 021B 6A1C 0F1A 6A1D 111B noadd12 if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfss else btfss endif goto add13 0223 0224 0225 0226 6A1C 0F1A 6A1D 111B noadd13 if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if i < 8 btfss else btfss endif goto add14 022E 022F 0230 0231 6A1C 0F1A 6A1D 111B movfp addwf movfp addwfc AARG+B0,wreg DPX+B2 AARG+B1,wreg DPX+B3 ;add lsb ;add msb AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 movfp addwf movfp addwfc AARG+B0,wreg DPX+B2 ;add lsb AARG+B1,wreg DPX+B3 ;add msb AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0 movfp addwf movfp addwfc AARG+B0,wreg DPX+B2 ;add lsb AARG+B1,wreg DPX+B3 ;add msb AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit
0216 941F 0217 C21C
4
3
021C 021D 021E 021F 0220 000D
1A1D 191B 191A 1919 1918
221 951F 0222 C227
0227 0228 0229 022A 022B 000E
1A1D 191B 191A 1919 1918
022C 961F 022D C232
DS00532B-page 45
4-229

noadd14 if i < 8 rlcf rrcf rrcf rrcf else rlcf rrcf rrcf rrcf rrcf endif i = i+1 if SIGNED rlcf rrcf rrcf rrcf rrcf endif return ;************************************************************************* include traject.asm ; Trajectory Generation ;************************************************************************* ; ; Trajectory Generation Routines ; ;************************************************************************* AARG+B1,W DPX+B3 DPX+B2 DPX+B1 AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0 ; rotate sign into carry bit ; for i < 8, no meaningful ; are in DPX+B0
0232 0233 0234 0235 0236 000F 0237 0238 0239 023A 023B
1A1D 191B 191A 1919 1918
1A1D 191B 191A 1919 1918
AARG+B1,W DPX+B3 DPX+B2 DPX+B1 DPX+B0
; since BARG is always made ; the last bit is known to be
023C 0002
;************************************************************************* ; NAME: doPreMove ; ; DESCRIPTION: doPreMove: CLR16 023D 2996 023E 2997 MOV24 023F 0240 0241 0242 0243 0244 0245 0246 0247 0248 0249 6A5B 4A5F 6A5C 4A60 6A5D 4A61 8F93 8693 8593 6AC2 4A94 INTEGRAL CLRF CLRF INTEGRAL+B0 INTEGRAL+B1 ; move buffer to MOVVAL ; ; ; ; ; ; get byte of NMOVVAL into w move to MOVVAL(B0) get byte of NMOVVAL into w move to MOVVAL(B1) get byte of NMOVVAL into w move to MOVVAL(B2)
NMOVVAL,MOVVAL MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF bcf bsf bsf movfp movpf NMOVVAL+B0,wreg wreg,MOVVAL+B0 NMOVVAL+B1,wreg wreg,MOVVAL+B1 NMOVVAL+B2,wreg wreg,MOVVAL+B2 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
024A 2951 MOV24 024B 6A55 024C 4A52 024D 6A56
clrf OPOSITION+B0 POSITION,OPOSITION+B1 MOVFP MOVPF MOVFP POSITION+B0,wreg wreg,OPOSITION+B1+B0 POSITION+B1,wreg
; initialize buffers
; get byte of POSITION into w ; move to OPOSITION+B1(B0) ; get byte of POSITION into w
DS00532B-page 46
4-230

024E 4A53 024F 6A57 0250 4A54 MOVPF MOVFP MOVPF wreg,OPOSITION+B1+B1 POSITION+B2,wreg wreg,OPOSITION+B1+B2 ; move to OPOSITION+B1(B1) ; get byte of POSITION into w ; move to OPOSITION+B1(B2)
MOV32 0251 0252 0253 0254 0255 0256 0257 0258 6A51 4AA4 6A52 4AA5 6A53 4AA6 6A54 4AA7
OPOSITION,MOVPBUF MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF OPOSITION+B0,wreg wreg,MOVPBUF+B0 OPOSITION+B1,wreg wreg,MOVPBUF+B1 OPOSITION+B2,wreg wreg,MOVPBUF+B2 OPOSITION+B3,wreg wreg,MOVPBUF+B3 ; ; ; ; ; ; ; ; get byte of OPOSITION move to MOVPBUF(B0) get byte of OPOSITION move to MOVPBUF(B1) get byte of OPOSITION move to MOVPBUF(B2) get byte of OPOSITION move to MOVPBUF(B3) into w into w into w into w
0259 2995 CLR16 025A 2967 025B 2968
clrf SATFLAG MOVTIME CLRF CLRF MOVTIME+B0 MOVTIME+B1
; clear move times
CLR16 025C 296A 025D 296B
T1 CLRF CLRF T1+B0 T1+B1
; 0 used as flag for no maximum
4
3
CLR16 025E 296C 025F 296D
T2 CLRF CLRF T2+B0 T2+B1
CLR16 0260 296E 0261 296F
TAU CLRF CLRF TAU+B0 TAU+B1
CLR32 0262 0263 0264 0265 29B0 29B1 29B2 29B3
MOVDEL CLRF CLRF CLRF CLRF MOVDEL+B0 MOVDEL+B1 MOVDEL+B2 MOVDEL+B3
; clear move discretization error
CLR16 0266 29B4 0267 29B5
PH2FLAT CLRF CLRF PH2FLAT+B0 PH2FLAT+B1
; clear phase 2 flat counter
0268 3391 0269 C2C5 pmode MOVFP24 026A 785F 026B 7960 026C 7A61
tstfsz goto
MODETYPE vmode
MOVVAL,TMP MOVFP MOVFP MOVFP MOVVAL+B0,TMP+B0 MOVVAL+B1,TMP+B1 MOVVAL+B2,TMP+B2 ; move MOVVAL(B0) to TMP(B0) ; move MOVVAL(B1) to TMP(B1) ; move MOVVAL(B2) to TMP(B2)
026D 971A 026E C276
btfss goto
TMP+B2,MSB mvpos
DS00532B-page 47
4-231

NEG24 026F 0270 0271 0272 0273 0274 0275 1318 1319 131A 290A 1518 1119 111A TMP COMF COMF COMF CLRF INCF ADDWFC ADDWFC TMP+B0 TMP+B1 TMP+B2 wreg TMP+B0 TMP+B1 TMP+B2
mvpos 0276 0277 tive 0278 0279 027A 291C 291D 291E 801C 811C SUB24 027B 027C 027D 027E 027F 0280 6A1C 0518 6A1D 0319 6A1E 031A clrf clrf MOVTMP+B0 MOVTMP+B1 ; calculate abs(MOVVAL) - 3 ; do immediate move if nega-
clrf MOVTMP+B2 bsf MOVTMP+B0,bit0 bsf MOVTMP+B0,bit1 MOVTMP,TMP MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB MOVTMP+B0,wreg TMP+B0 MOVTMP+B1,wreg TMP+B1 MOVTMP+B2,wreg TMP+B2 ; ; ; ; ; ; get sub get sub get sub lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of MOVTMP of TMP, save MOVTMP into TMP, save in MOVTMP into TMP, save in
0281 0282 0283 0284 0285 0286
971A C28E 2B90 2994 8D93 8E93 ADD24
btfss TMP+B2,MSB goto nonzero setf SERVOFLAG clrf MOVFLAG bcf MOVSTAT,bit5 bcf MOVSTAT,bit6 MOVVAL,POSITION MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVVAL+B0,wreg POSITION+B0 MOVVAL+B1,wreg POSITION+B1 MOVVAL+B2,wreg POSITION+B2
; check for zero move ; set servoflag to restore
0287 0288 0289 028A 028B 028C
6A5F 0F55 6A60 1156 6A61 1157
; ; ; ; ; ;
get add get add get add
lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of
of MOVVAL of POSITION, MOVVAL into POSITION, MOVVAL into POSITION,
028D 0002
return nonzero CLR32 MOVVBUF CLRF CLRF CLRF CLRF MOVVBUF+B0 MOVVBUF+B1 MOVVBUF+B2 MOVVBUF+B3
028E 028F 0290 0291
29A8 29A9 29AA 29AB
0292 6A61 0293 B580 0294 4A69 0295 29A3 MOV24 0296 0297 0298 0299 029A 029B 6A20 4AA0 6A21 4AA1 6A22 4AA2
movfp andlw movpf clrf VL,V MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF
MOVVAL+B2,wreg 0x80 wreg,MOVSIGN V+B3
; move sign
; create appropriate velocity ; acceleration limits from ; ; ; ; ; ; get byte of VL into w move to V(B0) get byte of VL into w move to V(B1) get byte of VL into w move to V(B2)
VL+B0,wreg wreg,V+B0 VL+B1,wreg wreg,V+B1 VL+B2,wreg wreg,V+B2
DS00532B-page 48
4-232
029C 299F MOV24 029D 029E 029F 02A0 02A1 02A2 6A23 4A9C 6A24 4A9D 6A25 4A9E
clrf AL,A MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF
A+B3
AL+B0,wreg wreg,A+B0 AL+B1,wreg wreg,A+B1 AL+B2,wreg wreg,A+B2
; ; ; ; ;
get byte of AL into w move to A(B0) get byte of AL into w move to A(B1) get byte of AL into w ; move to A(B2)
02A3 290A 02A4 3269 02A5 C2B8 NEG32 02A6 02A7 02A8 02A9 02AA 02AB 02AC 02AD 02AE 13A0 13A1 13A2 13A3 290A 15A0 11A1 11A2 11A3
clrf cpfsgt goto V COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC
wreg MOVSIGN minc
V+B0 V+B1 V+B2 V+B3 wreg V+B0 V+B1 V+B2 V+B3
4
3
NEG32 02AF 02B0 02B1 02B2 02B3 02B4 02B5 02B6 02B7 139C 139D 139E 139F 290A 159C 119D 119E 119F
A COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC A+B0 A+B1 A+B2 A+B3 wreg A+B0 A+B1 A+B2 A+B3
minc 02B8 2963 clrf MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF HMOVVAL+B0 MOVVAL,HMOVVAL+B1 MOVVAL+B0,wreg wreg,HMOVVAL+B1+B0 MOVVAL+B1,wreg wreg,HMOVVAL+B1+B1 MOVVAL+B2,wreg wreg,HMOVVAL+B1+B2 ; evaluate MOVVAL/2
02B9 02BA 02BB 02BC 02BD 02BE
6A5F 4A64 6A60 4A65 6A61 4A66
; ; ; ; ; ;
get byte of MOVVAL into w move to HMOVVAL+B1(B0) get byte of MOVVAL into w move to HMOVVAL+B1(B1) get byte of MOVVAL into w move to HMOVVAL+B1(B2)
RRC32 02BF 02C0 02C1 02C2 02C3 1A66 1966 1965 1964 1963
HMOVVAL RLCF RRCF RRCF RRCF RRCF HMOVVAL+B3,W HMOVVAL+B3 HMOVVAL+B2 HMOVVAL+B1 HMOVVAL+B0
; half move in Q8 ; move sign into carry bit
02C4 C2FE vmode 02C5 9F91 02C6 C306
goto
modeready
btfsc goto
MODETYPE,MSB tmode
; is it torque move?
DS00532B-page 49
4-233

02C7 2966 MOV24 02C8 02C9 02CA 02CB 02CC 02CD 6A5F 4A63 6A60 4A64 6A61 4A65 clrf HMOVVAL+B3 MOVVAL,HMOVVAL MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOVVAL+B0,wreg wreg,HMOVVAL+B0 MOVVAL+B1,wreg wreg,HMOVVAL+B1 MOVVAL+B2,wreg wreg,HMOVVAL+B2 ; compute final minus initial
; ; ; ; ; ;
get byte of MOVVAL into w move to HMOVVAL(B0) get byte of MOVVAL into w move to HMOVVAL(B1) get byte of MOVVAL into w move to HMOVVAL(B2)
02CE 9F61 02CF 2B66 SUB32 02D0 02D1 02D2 02D3 02D4 02D5 02D6 02D7 02D8 02D9 02DA 6AA8 0563 6AA9 0364 6AAA 0365 6AAB 0366 6A66 B580 4A69
btfsc MOVVAL+B2,MSB setf HMOVVAL+B3 MOVVBUF,HMOVVAL MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB MOVFP SUBWFB movfp andlw movpf clrf VL,V MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOVVBUF+B0,wreg HMOVVAL+B0 MOVVBUF+B1,wreg HMOVVAL+B1 MOVVBUF+B2,wreg HMOVVAL+B2 MOVVBUF+B3,wreg HMOVVAL+B3 HMOVVAL+B3,wreg 0x80 wreg,MOVSIGN V+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 MOVVBUF into w of HMOVVAL, save in MOVVBUF into w HMOVVAL, save in MOVVBUF into w HMOVVAL, save in MOVVBUF into w HMOVVAL, save in
02DB 29A3 MOV24 02DC 02DD 02DE 02DF 02E0 02E1 6A20 4AA0 6A21 4AA1 6A22 4AA2
; create appropriate velocity and ; acceleration limits from move sign ; ; ; ; ; ; get byte of VL into w move to V(B0) get byte of VL into w move to V(B1) get byte of VL into w move to V(B2)
VL+B0,wreg wreg,V+B0 VL+B1,wreg wreg,V+B1 VL+B2,wreg wreg,V+B2
02E2 299F MOV24 02E3 02E4 02E5 02E6 02E7 02E8 6A23 4A9C 6A24 4A9D 6A25 4A9E
clrf AL,A MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF
A+B3
AL+B0,wreg wreg,A+B0 AL+B1,wreg wreg,A+B1 AL+B2,wreg wreg,A+B2
; ; ; ; ; ;
get byte of AL into w move to A(B0) get byte of AL into w move to A(B1) get byte of AL into w move to A(B2)
02E9 290A 02EA 3269 02EB C2FE NEG32 02EC 02ED 02EE 02EF 02F0 02F1 02F2 02F3 02F4 13A0 13A1 13A2 13A3 290A 15A0 11A1 11A2 11A3 V
clrf cpfsgt goto
wreg MOVSIGN modeready
COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC
V+B0 V+B1 V+B2 V+B3 wreg V+B0 V+B1 V+B2 V+B3
NEG32 02F5 139C 02F6 139D
A COMF COMF A+B0 A+B1
DS00532B-page 50
4-234

02F7 02F8 02F9 02FA 02FB 02FC 02FD 139E 139F 290A 159C 119D 119E 119F COMF A+B2 COMF A+B3 CLRF wreg INCF A+B0 ADDWFC A+B1 ADDWFC A+B2 ADDWFC A+B3 modeready clrf MOVVAL+B3 btfsc MOVVAL+B2,MSB setf MOVVAL+B3 setf SERVOFLAG ; set servoflag to restore servo ; if stopped
02FE 2962 02FF 9F61 0300 2B62 0301 2B90
if
_PICMASTER_DEBUG
0302 6ABC 0303 08BD 0304 4ABB
;**************************************************************** ; For PICMASTER Debug/servo tuning puporses only Purposes Only ; testCapCount movfp CAPCOUNT+B0,wreg iorwf CAPCOUNT+B1,W movpf wreg,CAPFLAG ;************************************************************************* endif return tmode MOV16 ; torque/voltage mode ; set new commanded value ; ; ; ; get byte of MOVVAL+B1 into w move to YPWM(B0) get byte of MOVVAL+B1 into w move to YPWM(B1)
4
3
0305 0002
MOVVAL+B1,YPWM MOVFP MOVWF MOVFP MOVWF MOVVAL+B1+B0,wreg YPWM+B0 MOVVAL+B1+B1,wreg YPWM+B1
0306 0307 0308 0309
6A60 0188 6A61 0189
030A 030B 030C 030D
2990 E50B 2994 8D93 if
030E C302 else
clrf SERVOFLAG call doTorque clrf MOVFLAG bcf MOVSTAT,bit5 _PICMASTER_DEBUG goto testCapCount return
; disable servo ; set pwm duty cycle
endif ;************************************************************************** ;************************************************************************* ; 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 when c ; 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 ; ; ________________ ; / \
DS00532B-page 51
4-235

; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; doMove INC16 030F 290A 0310 1567 0311 1168 0312 E468 0313 3391 0314 C429 0315 6AC2 0316 3194 0317 C3DB phase1 0318 0319 031A 031B 031C 031D 031E 031F 7CB0 7DB1 7EB2 7FB3 6A51 4AB0 6A52 4AB1 MOVFP32 MOVFP MOVFP MOVFP MOVFP MOV32 MOVFP MOVPF MOVFP MOVPF 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 ; save previous discretization error ; move MOVDEL(B0) to MOVTMP(B0) ; move MOVDEL(B1) to MOVTMP(B1) ; move MOVDEL(B2) to MOVTMP(B2) ; move MOVDEL(B3) to MOVTMP(B3) ; test if half move ; get byte of OPOSITION into w ; move to MOVDEL(B0) ; get byte of OPOSITION into w ; move to MOVDEL(B1) MOVTIME CLRF INCF ADDWFC wreg (MOVTIME)+B0 (MOVTIME)+B1 ; evaluate iterative equations ; increment move time /\ / \ / \ / \ / \ 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
call doPosVel tstfsz MODETYPE goto vmove pmove movfp ONE,wreg cpfseq MOVFLAG goto phase2
; test if in phase1
DS00532B-page 52
4-236

0320 0321 0322 0323 0324 0325 0326 0327 0328 0329 032A 032B 032C 032D 032E 032F 0330 0331 0332 0333 0334 0335 0336 0337 0338 0339 033A 033B 033C 033D 033E 6A53 4AB2 6A54 4AB3 6A63 0FB0 6A64 11B1 6A65 11B2 6A66 11B3 6AA4 05B0 6AA5 03B1 6AA6 03B2 6AA7 03B3 9769 C33F 13B0 13B1 13B2 13B3 290A 15B0 11B1 11B2 11B3 MOVFP MOVPF MOVFP MOVPF ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC SUB32 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB MOVFP SUBWFB btfss goto NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC mpos1 btfss goto TFSZ16 MOVFP IORWF TSTFSZ OPOSITION+B2,wreg wreg,MOVDEL+B2 OPOSITION+B3,wreg wreg,MOVDEL+B3 HMOVVAL,MOVDEL HMOVVAL+B0,wreg MOVDEL+B0 HMOVVAL+B1,wreg MOVDEL+B1 HMOVVAL+B2,wreg MOVDEL+B2 HMOVVAL+B3,wreg MOVDEL+B3 MOVPBUF,MOVDEL MOVPBUF+B0,wreg MOVDEL+B0 MOVPBUF+B1,wreg MOVDEL+B1 MOVPBUF+B2,wreg MOVDEL+B2 MOVPBUF+B3,wreg MOVDEL+B3 MOVSIGN,MSB mpos1 MOVDEL MOVDEL+B0 MOVDEL+B1 MOVDEL+B2 MOVDEL+B3 wreg MOVDEL+B0 MOVDEL+B1 MOVDEL+B2 MOVDEL+B3 MOVDEL+B3,MSB speedup T1 T1+B0,wreg T1+B1,W wreg ; ; ; ; get byte of OPOSITION into w move to MOVDEL(B2) get byte of OPOSITION into w move to MOVDEL(B3)
; get lowest byte of HMOVVAL into w ; add lowest byte of MOVDEL, save in ; get 2nd byte of HMOVVAL into w ; add 2nd byte of MOVDEL, save in ; get 3rd byte of HMOVVAL into w ; add 3rd byte of MOVDEL, save in ; get 4th byte of HMOVVAL into w ; add 4th byte of MOVDEL, save in ; ; ; ; ; ; ; ; 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 MOVPBUF into of MOVDEL, save MOVPBUF into w MOVDEL, save in MOVPBUF into w MOVDEL, save in MOVPBUF into w MOVDEL, save in
4
3
033F 97B3 0340 C3A5 0341 6A6A 0342 086B 0343 330A
; continue to speed up if in ; if T1=0, maximum velocity not
; reached, ; has been set in speedup 0344 C378 0345 0346 0347 0348 0349 034A 034B 034C 034D 034E 034F 0350 0351 0352 0353 0354 0355 0356 0357 0358 0359 139C 139D 139E 139F 290A 159C 119D 119E 119F 6AB0 0F1C 6AB1 111D 6AB2 111E 6AB3 111F 971F C36E 2B6C 2B6D goto NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC btfss goto setf setf NEG32 t2net1 A A+B0 A+B1 A+B2 A+B3 wreg A+B0 A+B1 A+B2 A+B3 MOVDEL,MOVTMP MOVDEL+B0,wreg MOVTMP+B0 MOVDEL+B1,wreg MOVTMP+B1 MOVDEL+B2,wreg MOVTMP+B2 MOVDEL+B3,wreg MOVTMP+B3 MOVTMP+B3,MSB triok T2+B0 T2+B1 A ; negate A for speeddown
; ; ; ; ; ; ; ; ; ; ; ;
test x-y < 0 get lowest byte of MOVDEL into add lowest byte of MOVTMP, save get 2nd byte of MOVDEL into w add 2nd byte of MOVTMP, save in get 3rd byte of MOVDEL into w add 3rd byte of MOVTMP, save in get 4th byte of MOVDEL into w add 4th byte of MOVTMP, save in if new discretization error larger, backup to define T2, otherwise ok set T2=-1 for backup
; negate A to undo
DS00532B-page 53
4-237
035A 035B 035C 035D 035E 035F 0360 0361 0362 0363 0364 0365 0366 0367 0368 0369 036A 036B 036C 036D
139C 139D 139E 139F 290A 159C 119D 119E 119F E48A 139C 139D 139E 139F 290A 159C 119D 119E 119F E468
036E 036F 0370 0371 0372 0373 0374 0375 0376 0377
6A67 0F6C 6A68 116D 6A6C 016A 6A6D 016B 1594 C3CE t2net1
COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC call NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC call triok ADD16 MOVFP ADDWF MOVFP ADDWFC MOV16 MOVFP MOVWF MOVFP MOVWF incf goto setf setf ADD16 MOVFP ADDWF MOVFP ADDWFC MOVFP32 MOVFP MOVFP MOVFP MOVFP RLC32 BCF RLCF RLCF RLCF RLCF ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC ADD32 MOVFP ADDWF MOVFP ADDWFC
A+B0 A+B1 A+B2 A+B3 wreg A+B0 A+B1 A+B2 A+B3 undoPosVel A A+B0 A+B1 A+B2 A+B3 wreg A+B0 A+B1 A+B2 A+B3 doPosVel MOVTIME,T2 MOVTIME+B0,wreg T2+B0 MOVTIME+B1,wreg T2+B1 T2,T1 T2+B0,wreg T1+B0 T2+B1,wreg T1+B1 MOVFLAG mvok T2+B0 T2+B1 MOVTIME,T2 MOVTIME+B0,wreg T2+B0 MOVTIME+B1,wreg T2+B1 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 TMP+B1,wreg MOVTMP+B1 TMP+B2,wreg MOVTMP+B2 TMP+B3,wreg MOVTMP+B3 MOVDEL,MOVTMP MOVDEL+B0,wreg MOVTMP+B0 MOVDEL+B1,wreg MOVTMP+B1
; negate A again for speeddown
; and reevaluate iterative equations ; add time to T2 lowest byte of MOVTIME into w lowest byte of T2, save in 2nd byte of MOVTIME into w 2nd byte of T2, save in T2(B1) w w for move
; ; ; ; ; ; ; ; ; ;
get add get add
get byte of T2 into move to T1(B0) get byte of T2 into move to T1(B1) increment move flag execute last phase1
0378 2B6C 0379 2B6D 037A 037B 037C 037D 037E 037F 0380 0381 0382 0383 0384 0385 0386 0387 0388 0389 038A 038B 038C 038D 038E 038F 0390 0391 0392 6A67 0F6C 6A68 116D 781C 791D 7A1E 7B1F 8804 1B1C 1B1D 1B1E 1B1F 6A18 0F1C 6A19 111D 6A1A 111E 6A1B 111F 6AB0 0F1C 6AB1 111D
; set T2=-1 for backup ; ; ; ; ; ; ; ; ; ; add time to T2 get lowest byte of MOVTIME add lowest byte of T2, save get 2nd byte of MOVTIME into add 2nd byte of T2, save in T2(B1) test if 3x-y < 0 move MOVTMP(B0) to TMP(B0) move MOVTMP(B1) to TMP(B1) move MOVTMP(B2) to TMP(B2) move MOVTMP(B3) to TMP(B3)
; ; ; ; ; ; ; ; ; ; ; ;
get add get add 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 lowest byte lowest byte 2nd byte of 2nd byte of
of TMP into of MOVTMP, TMP into w MOVTMP, save TMP into w MOVTMP, save TMP into w MOVTMP, save of MOVDEL of MOVTMP, MOVDEL into MOVTMP, save
DS00532B-page 54
4-238

0393 0394 0395 0396 0397 0398 0399 039A 6AB2 111E 6AB3 111F 971F C39B 2BB4 2BB5 trapok 039B 039C 039D 039E 039F 03A0 03A1 03A2 03A3 03A4 6A6C 0FB4 6A6D 11B5 6A6A 05B4 6A6B 03B5 1594 C3CE ADD16 T2,PH2FLAT MOVFP T2+B0,wreg ADDWF PH2FLAT+B0 MOVFP T2+B1,wreg ADDWFC PH2FLAT+B1 SUB16 T1,PH2FLAT MOVFP T1+B0,wreg SUBWF PH2FLAT+B0 MOVFP T1+B1,wreg SUBWFB PH2FLAT+B1 incf MOVFLAG goto mvok speedup MOVFP32 V,MOVTMP MOVFP V+B0,MOVTMP+B0 MOVFP V+B1,MOVTMP+B1 MOVFP V+B2,MOVTMP+B2 MOVFP V+B3,MOVTMP+B3 SUB32 MOVVBUF,MOVTMP MOVFP MOVVBUF+B0,wreg SUBWF MOVTMP+B0 MOVFP MOVVBUF+B1,wreg SUBWFB MOVTMP+B1 MOVFP MOVVBUF+B2,wreg SUBWFB MOVTMP+B2 MOVFP MOVVBUF+B3,wreg SUBWFB MOVTMP+B3 btfss MOVSIGN,MSB goto mpos NEG32 MOVTMP COMF MOVTMP+B0 COMF MOVTMP+B1 COMF MOVTMP+B2 COMF MOVTMP+B3 CLRF wreg INCF MOVTMP+B0 ADDWFC MOVTMP+B1 ADDWFC MOVTMP+B2 ADDWFC MOVTMP+B3 mpos 03BC 971F 03BD C3CE 03BE 6A6A 03BF 086B 03C0 330A 03C1 C3CE 03C2 E48A 03C3 03C4 03C5 03C6 03C7 03C8 03C9 299C 299D 299E 299F E468 2B6A 2B6B btfss goto TFSZ16 MOVFP IORWF TSTFSZ goto call CLR32 CLRF CLRF CLRF CLRF call setf setf ADD16 MOVFP ADDWF MOVTMP+B3,MSB mvok T1 T1+B0,wreg T1+B1,W wreg mvok undoPosVel A A+B0 A+B1 A+B2 A+B3 doPosVel T1+B0 T1+B1 MOVTIME,T1 MOVTIME+B0,wreg T1+B0 ; if not, execute move ; if so, check to see if T1 ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; get add get add lowest byte lowest byte 2nd byte of 2nd byte of of T2 into w of PH2FLAT, T2 into w PH2FLAT, MOVFP ADDWFC MOVFP ADDWFC btfss goto setf setf MOVDEL+B2,wreg MOVTMP+B2 MOVDEL+B3,wreg MOVTMP+B3 MOVTMP+B3,MSB trapok PH2FLAT+B0 PH2FLAT+B1 ; ; ; ; ; ; get 3rd byte of MOVDEL into add 3rd byte of MOVTMP, save get 4th byte of MOVDEL into add 4th byte of MOVTMP, save if new discretization error take one more flat step
get lowest byte of T1 into w sub lowest byte of PH2FLAT, get 2nd byte of T1 into w sub 2nd byte of PH2FLAT, increment move flag for execute last phase1 move test move move move move get sub get sub get sub get sub if maximum velocity V(B0) to MOVTMP(B0) V(B1) to MOVTMP(B1) V(B2) to MOVTMP(B2) V(B3) to MOVTMP(B3) lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of 4th byte of of MOVVBUF of MOVTMP, MOVVBUF into MOVTMP, save MOVVBUF into MOVTMP, save MOVVBUF into MOVTMP, save in
03A5 03A6 03A7 03A8 03A9 03AA 03AB 03AC 03AD 03AE 03AF 03B0 03B1 03B2 03B3 03B4 03B5 03B6 03B7 03B8 03B9 03BA 03BB
7CA0 7DA1 7EA2 7FA3 6AA8 051C 6AA9 031D 6AAA 031E 6AAB 031F 9769 C3BC 131C 131D 131E 131F 290A 151C 111D 111E 111F
4
3
; already been set ; if not, backup and redo ; equations, resulting in an
; maximum speed <= VL ; evaluate T1
03CA 6A67 03CB 0F6A
; get lowest byte of MOVTIME ; add lowest byte of T1, save
DS00532B-page 55
4-239

03CC 6A68 03CD 116B mvok 03CE 03CF 03D0 03D1 03D2 03D3 03D4 03D5 03D6 03D7 03D8 03D9 03DA 6AA5 4A55 6AA6 4A56 6AA7 4A57 6AA8 4A58 6AA9 4A59 6AAA 4A5A 0002 MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF return phase2 TFSZ16 MOVFP IORWF TSTFSZ goto TFSZ32 MOVFP IORWF IORWF IORWF TSTFSZ goto clrf bcf bcf CLR32 CLRF CLRF CLRF CLRF MOV16 MOVFP MOVWF MOVFP MOVWF MOV32 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF ADD24 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC goto flat 03FF 2B1C 0400 2B1D 0401 6A1C 0402 0FB4 setf setf ADD16 MOVFP ADDWF MOVTMP+B0 MOVTMP+B1 MOVTMP,PH2FLAT MOVTMP+B0,wreg PH2FLAT+B0 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 ; ; ; ; ; ; ; ; ; ; ; ; ; ; move Q8 calculated position get byte of MOVPBUF+B1 into move to POSITION(B0) get byte of MOVPBUF+B1 into move to POSITION(B1) get byte of MOVPBUF+B1 into move to POSITION(B2) move Q0 calculated velocity get byte of MOVVBUF+B0 into move to VELOCITY(B0) get byte of MOVVBUF+B0 into move to VELOCITY(B1) get byte of MOVVBUF+B0 into move to VELOCITY(B2) MOVFP ADDWFC MOVTIME+B1,wreg T1+B1 ; get 2nd byte of MOVTIME into ; add 2nd byte of T1, save in
03DB 03DC 03DD 03DE 03DF 03E0 03E1 03E2 03E3 03E4 03E5 03E6 03E7 03E8 03E9 03EA 03EB 03EC 03ED 03EE 03EF 03F0 03F1 03F2 03F3 03F4 03F5 03F6 03F7 03F8 03F9 03FA 03FB 03FC 03FD 03FE
6AB4 08B5 330A C3FF 6AA8 08A9 08AA 08AB 330A C41C 2994 8E93 8D93 299C 299D 299E 299F 6A67 016E 6A68 016F 6A51 4AA4 6A52 4AA5 6A53 4AA6 6A54 4AA7 6A5F 0FA5 6A60 11A6 6A61 11A7 C41C
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 MOVSTAT,bit6 MOVSTAT,bit5 A A+B0 A+B1 A+B2 A+B3 MOVTIME,TAU MOVTIME+B0,wreg TAU+B0 MOVTIME+B1,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 MOVVAL+B1,wreg MOVPBUF+B1+B1 MOVVAL+B2,wreg MOVPBUF+B1+B2 mready
; 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,
; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ;
get byte of MOVTIME into w move to TAU(B0) get byte of MOVTIME into w move to TAU(B1) execute last move to P(0)+MOVVAL get byte of OPOSITION into w move to MOVPBUF(B0) get byte of OPOSITION into w move to MOVPBUF(B1) get byte of OPOSITION into w move to MOVPBUF(B2) get byte of OPOSITION into w move to MOVPBUF(B3) get add get add get add lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of of MOVVAL into w of MOVPBUF+B1, save MOVVAL into w MOVPBUF+B1, save in MOVVAL into w MOVPBUF+B1, save in
; decrement by one use DEC16 ; get lowest byte of MOVTMP into w ; add lowest byte of PH2FLAT, save in
DS00532B-page 56
4-240

0403 6A1D 0404 11B5 0405 0406 0407 0408 0409 040A 040B 040C 040D 040E 040F 0410 0411 0412 0413 0414 0415 0416 0417 0418 0419 041A 041B 6AB4 08B5 330A C41C 299F 6A23 4A9C 6A24 4A9D 6A25 4A9E 290A 3169 C41C 139C 139D 139E 139F 290A 159C 119D 119E 119F mready 041C 041D 041E 041F 0420 0421 0422 0423 0424 0425 0426 0427 0428 6AA5 4A55 6AA6 4A56 6AA7 4A57 6AA8 4A58 6AA9 4A59 6AAA 4A5A 0002 vmove 0429 042A 042B 042C 042D 042E 042F 0430 0431 0432 0433 0434 0435 0436 0437 0438 0439 043A 043B 043C 7C5F 7D60 7E61 7F62 6AA8 051C 6AA9 031D 6AAA 031E 6AAB 031F 9769 C440 131C 131D 131E 131F 290A 151C MOVFP32 MOVFP MOVFP MOVFP MOVFP SUB32 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB MOVFP SUBWFB btfss goto NEG32 COMF COMF COMF COMF CLRF INCF MOVVAL,MOVTMP MOVVAL+B0,MOVTMP+B0 MOVVAL+B1,MOVTMP+B1 MOVVAL+B2,MOVTMP+B2 MOVVAL+B3,MOVTMP+B3 MOVVBUF,MOVTMP MOVVBUF+B0,wreg MOVTMP+B0 MOVVBUF+B1,wreg MOVTMP+B1 MOVVBUF+B2,wreg MOVTMP+B2 MOVVBUF+B3,wreg MOVTMP+B3 MOVSIGN,MSB vmpos MOVTMP MOVTMP+B0 MOVTMP+B1 MOVTMP+B2 MOVTMP+B3 wreg MOVTMP+B0 ; ; ; ; ; ; ; ; ; ; ; ; ; test move move move move get sub get sub get sub get sub if final velocity reached MOVVAL(B0) to MOVTMP(B0) MOVVAL(B1) to MOVTMP(B1) MOVVAL(B2) to MOVTMP(B2) MOVVAL(B3) to MOVTMP(B3) of MOVVBUF into w of MOVTMP, save in MOVVBUF into w MOVTMP, save in MOVVBUF into w MOVTMP, save in MOVVBUF into w MOVTMP, save in 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 MOVPBUF+B1 into w move to POSITION(B0) get byte of MOVPBUF+B1 into w move to POSITION(B1) get byte of MOVPBUF+B1 into w move to POSITION(B2) get byte of MOVVBUF+B0 into w move to VELOCITY(B0) get byte of MOVVBUF+B0 into w move to VELOCITY(B1) get byte of MOVVBUF+B0 into w move to VELOCITY(B2) MOVFP ADDWFC TFSZ16 MOVFP IORWF TSTFSZ goto clrf MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF clrf cpfseq goto NEG32 COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC MOVTMP+B1,wreg PH2FLAT+B1 PH2FLAT PH2FLAT+B0,wreg PH2FLAT+B1,W wreg mready A+B3 AL,A AL+B0,wreg wreg,A+B0 AL+B1,wreg wreg,A+B1 AL+B2,wreg wreg,A+B2 wreg MOVSIGN mready A A+B0 A+B1 A+B2 A+B3 wreg A+B0 A+B1 A+B2 A+B3 ; get 2nd byte of MOVTMP into w ; add 2nd byte of PH2FLAT, save in
; begin speed down section ; ; ; ; ; ; get byte of AL into w move to A(B0) get byte of AL into w move to A(B1) get byte of AL into w move to A(B2)
4
3
lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of 4th byte of 4th byte of
DS00532B-page 57
4-241

043D 111D 043E 111E 043F 111F vmpos 0440 971F 0441 C45B 0442 0443 0444 0445 0446 0447 0448 0449 044A 044B 044C 044D 299C 299D 299E 299F 6A5F 4AA8 6A60 4AA9 6A61 4AAA 6A62 4AAB btfss goto CLR32 CLRF CLRF CLRF CLRF MOV32 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF clrf bcf MOV16 MOVFP MOVWF MOVFP MOVWF TFSZ32 MOVFP IORWF IORWF IORWF TSTFSZ goto bcf vmoveok 045B 045C 045D 045E 045F 0460 0461 0462 0463 0464 0465 0466 0467 6AA5 4A55 6AA6 4A56 6AA7 4A57 6AA8 4A58 6AA9 4A59 6AAA 4A5A 0002 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 MOVPBUF+B1 into w move to POSITION(B0) get byte of MOVPBUF+B1 into w move to POSITION(B1) get byte of MOVPBUF+B1 into w move to POSITION(B2) get byte of MOVVBUF+B0 into w move to VELOCITY(B0) get byte of MOVVBUF+B0 into w move to VELOCITY(B1) get byte of MOVVBUF+B0 into w move to VELOCITY(B2) MOVTMP+B3,MSB vmoveok A A+B0 A+B1 A+B2 A+B3 MOVVAL,MOVVBUF MOVVAL+B0,wreg wreg,MOVVBUF+B0 MOVVAL+B1,wreg wreg,MOVVBUF+B1 MOVVAL+B2,wreg wreg,MOVVBUF+B2 MOVVAL+B3,wreg wreg,MOVVBUF+B3 MOVFLAG MOVSTAT,bit5 MOVTIME,TAU MOVTIME+B0,wreg TAU+B0 MOVTIME+B1,wreg TAU+B1 MOVVAL MOVVAL+B0,wreg MOVVAL+B1,W MOVVAL+B2,W MOVVAL+B3,W wreg vmoveok MOVSTAT,bit6 ; if not, continue ; if so, set A=0 and continue with ADDWFC ADDWFC ADDWFC MOVTMP+B1 MOVTMP+B2 MOVTMP+B3
044E 2994 044F 8D93 0450 0451 0452 0453 0454 0455 0456 0457 0458 0459 045A 6A67 016E 6A68 016F 6A5F 0860 0861 0862 330A C45B 8E93
; ; ; ; ; ; ; ; ; ; ; ; ; ; ; ;
move unless the final velocity get byte of MOVVAL into w move to MOVVBUF(B0) get byte of MOVVAL into w move to MOVVBUF(B1) get byte of MOVVAL into w move to MOVVBUF(B2) get byte of MOVVAL into w move to MOVVBUF(B3) is zero. clear MOVFLAG clear move in progress flag get byte of MOVTIME into w move to TAU(B0) get byte of MOVTIME into w move to TAU(B1)
; if final velocity is zero, clear ; motion status flag
DS00532B-page 58
4-242

;****************************************************************** ;****************************************************************** ; NAME: doPosVel ; ; DESCRIPTION: Evaluates the iterative equations for trapezoidal ; 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 ; being executed. ; doPosVel ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC MOVFP32 MOVFP MOVFP MOVFP MOVFP RRC32 RLCF RRCF RRCF RRCF RRCF ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC return MOVVBUF,MOVPBUF MOVVBUF+B0,wreg MOVPBUF+B0 MOVVBUF+B1,wreg MOVPBUF+B1 MOVVBUF+B2,wreg MOVPBUF+B2 MOVVBUF+B3,wreg MOVPBUF+B3 A,MOVVBUF A+B0,wreg MOVVBUF+B0 A+B1,wreg MOVVBUF+B1 A+B2,wreg MOVVBUF+B2 A+B3,wreg MOVVBUF+B3 A,MOVTMP A+B0,MOVTMP+B0 A+B1,MOVTMP+B1 A+B2,MOVTMP+B2 A+B3,MOVTMP+B3 MOVTMP MOVTMP+B3,W MOVTMP+B3 MOVTMP+B2 MOVTMP+B1 MOVTMP+B0 MOVTMP,MOVPBUF MOVTMP+B0,wreg MOVPBUF+B0 MOVTMP+B1,wreg MOVPBUF+B1 MOVTMP+B2,wreg MOVPBUF+B2 MOVTMP+B3,wreg MOVPBUF+B3 ; ; ; ; ; ; ; ; ; P(k-1)+V(k-1) get lowest byte add lowest byte get 2nd byte of add 2nd byte of get 3rd byte of add 3rd byte of get 4th byte of add 4th byte of
0468 0469 046A 046B 046C 046D 046E 046F
6AA8 0FA4 6AA9 11A5 6AAA 11A6 6AAB 11A7
of MOVVBUF into w of MOVPBUF, save in MOVVBUF into w MOVPBUF, save in MOVVBUF into w MOVPBUF, save in MOVVBUF into w MOVPBUF, save in
4
3
0470 0471 0472 0473 0474 0475 0476 0477
6A9C 0FA8 6A9D 11A9 6A9E 11AA 6A9F 11AB
; ; ; ; ; ; ; ; ; ; ; ; ;
get add get add get add get add
; V(k)=V(k-1)+A lowest byte of A into w lowest byte of MOVVBUF, save in 2nd byte of A into w 2nd byte of MOVVBUF, save in 3rd byte of A into w 3rd byte of MOVVBUF, save in 4th byte of A into w 4th byte of MOVVBUF, save in
0478 0479 047A 047B
7C9C 7D9D 7E9E 7F9F
compute A/2 move A(B0) to move A(B1) to move A(B2) to move A(B3) to
MOVTMP(B0) MOVTMP(B1) MOVTMP(B2) MOVTMP(B3)
047C 047D 047E 047F 0480
1A1F 191F 191E 191D 191C
; move sign into carry bit
0481 0482 0483 0484 0485 0486 0487 0488
6A1C 0FA4 6A1D 11A5 6A1E 11A6 6A1F 11A7
; ; ; ; ; ; ; ;
get add get add get add get add
; P(k)=P(k-1)+V(k-1)+A/2, lowest byte of MOVTMP into w lowest byte of MOVPBUF, save in 2nd byte of MOVTMP into w 2nd byte of MOVPBUF, save in 3rd byte of MOVTMP into w 3rd byte of MOVPBUF, save in 4th byte of MOVTMP into w 4th byte of MOVPBUF, save in )
0489 0002
DS00532B-page 59
4-243
;******************************************************************* ;***************************************************************** ; NAME: undoPosVel ; ; DESCRIPTION: Backward iteration of the equations for trapezoidal ; 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 ; being executed. This routine is used to reverse a ; to be made beyond a decision point. ; undoPosVel SUB32 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB MOVFP SUBWFB SUB32 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB MOVFP SUBWFB MOVFP32 MOVFP MOVFP MOVFP MOVFP RRC32 RLCF RRCF RRCF RRCF RRCF SUB32 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB MOVFP SUBWFB return A,MOVVBUF A+B0,wreg MOVVBUF+B0 A+B1,wreg MOVVBUF+B1 A+B2,wreg MOVVBUF+B2 A+B3,wreg MOVVBUF+B3 ; ; ; ; ; ; ; ; ; V(k-1)=V(k)-A get lowest byte sub lowest byte get 2nd byte of sub 2nd byte of get 3rd byte of sub 3rd byte of get 4th byte of sub 4th byte of P(k)-V(k-1) get lowest byte sub lowest byte get 2nd byte of sub 2nd byte of get 3rd byte of sub 3rd byte of get 4th byte of sub 4th byte of compute A/2 move A(B0) to move A(B1) to move A(B2) to move A(B3) to
048A 048B 048C 048D 048E 048F 0490 0491
6A9C 05A8 6A9D 03A9 6A9E 03AA 6A9F 03AB
of A into w of MOVVBUF, save in A into w MOVVBUF, save in A into w MOVVBUF, save in A into w MOVVBUF, save in
0492 0493 0494 0495 0496 0497 0498 0499
6AA8 05A4 6AA9 03A5 6AAA 03A6 6AAB 03A7
MOVVBUF,MOVPBUF ; MOVVBUF+B0,wreg ; MOVPBUF+B0 ; MOVVBUF+B1,wreg ; MOVPBUF+B1 ; MOVVBUF+B2,wreg ; MOVPBUF+B2 ; MOVVBUF+B3,wreg ; MOVPBUF+B3 ; A,MOVTMP A+B0,MOVTMP+B0 A+B1,MOVTMP+B1 A+B2,MOVTMP+B2 A+B3,MOVTMP+B3 MOVTMP MOVTMP+B3,W MOVTMP+B3 MOVTMP+B2 MOVTMP+B1 MOVTMP+B0 MOVTMP,MOVPBUF MOVTMP+B0,wreg MOVPBUF+B0 MOVTMP+B1,wreg MOVPBUF+B1 MOVTMP+B2,wreg MOVPBUF+B2 MOVTMP+B3,wreg MOVPBUF+B3 ; ; ; ; ;
of MOVVBUF into w of MOVPBUF, save in MOVVBUF into w MOVPBUF, save in MOVVBUF into w MOVPBUF, save in MOVVBUF into w MOVPBUF, save in
049A 049B 049C 049D
7C9C 7D9D 7E9E 7F9F
MOVTMP(B0) MOVTMP(B1) MOVTMP(B2) MOVTMP(B3)
049E 049F 04A0 04A1 04A2
1A1F 191F 191E 191D 191C
; move sign into carry bit
04A3 04A4 04A5 04A6 04A7 04A8 04A9 04AA
6A1C 05A4 6A1D 03A5 6A1E 03A6 6A1F 03A7
; ; ; ; ; ; ; ; ;
P(k-1)=P(k)-V(k-1)-A/2, get lowest byte of MOVTMP into w sub lowest byte of MOVPBUF, save in get 2nd byte of MOVTMP into w sub 2nd byte of MOVPBUF, save in get 3rd byte of MOVTMP into w sub 3rd byte of MOVPBUF, save in get 4th byte of MOVTMP into w sub 4th byte of MOVPBUF, save in
04AB 0002
DS00532B-page 60
4-244

;**************************************************************** if _SERVO_PID include pid.asm ; PID Algorithm ;***************************************************************** ; PID Servo Implementation ; ; Implement Y=KP*U0+KI*INTEGRAL+KV*(U0-U1) ; ;******************************************************************
;******************************************************************* ; NAME: doServo ; ; DESCRIPTION: Performs the servo loop calculations. ; doServo: MOV16 MOVFP MOVWF MOVFP MOVWF LOADAB MOVFP MOVFP MOVFP MOVFP call MOVPF32 MOVPF MOVPF MOVPF MOVPF clrf cpfsgt call LOADAB MOVFP MOVFP MOVFP MOVFP call ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC POSERROR,U0 POSERROR+B0,wreg U0+B0 POSERROR+B1,wreg U0+B1 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 wreg 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 Y+B0 DPX+B1,wreg Y+B1 DPX+B2,wreg Y+B2 DPX+B3,wreg Y+B3 ; save new position error in ; get byte of POSERROR into w ;move to U0(B0) ; get byte of POSERROR into w ; move to U0(B1) ; ; ; ; ; ; ; ; ; ; ; ; compute load lo load hi load lo load hi KP*U0 byte of byte of byte of byte of
04AC 04AD 04AE 04AF
6A79 0184 6A7A 0185
4
3
04B0 04B1 04B2 04B3 04B4 04B5 04B6 04B7 04B8 04B9 04BA 04BB
7C84 7D85 7E26 7F27 E12B 5880 5981 5A82 5B83 290A 3295 E552
U0 U0 KP KP
to to to to
AARG AARG BARG BARG
Y=KP*U0 move DPX(B0) to Y(B0) move DPX(B1) to Y(B1) move DPX(B2) to Y(B2) move DPX(B3) to Y(B3) if previous output saturated, do not accumulate integrator
04BC 04BD 04BE 04BF 04C0
7C96 7D97 7E2A 7F2B E12B
; ; ; ; ;
compute load lo load hi load lo load hi
KI*INTEGRAL byte of INTEGRAL to AARG byte of INTEGRAL to AARG byte of KI to BARG byte of KI to BARG
04C1 04C2 04C3 04C4 04C5 04C6 04C7 04C8
6A18 0F80 6A19 1181 6A1A 1182 6A1B 1183
; ; ; ; ; ; ; ; ;
Y=KP*U0+KI*INTEGRAL get lowest byte of DPX into w add lowest byte of Y, save in Y(B0) get 2nd byte of DPX into w add 2nd byte of Y, save in Y(B1) get 3rd byte of DPX into w add 3rd byte of Y, save in Y(B2) get 4th byte of DPX into w add 4th byte of Y, save in Y(B3)
04C9 7C84 04CA 7D85
MOVFP16 U0,AARG MOVFP U0+B0,AARG+B0 MOVFP U0+B1,AARG+B1 SUB16 MOVFP SUBWF MOVFP SUBWFB U1,AARG U1+B0,wreg AARG+B0 U1+B1,wreg AARG+B1
; compute KV*(U0-U1) ; move U0(B0) to AARG(B0) ; move U0(B1) to AARG(B1)
04CB 04CC 04CD 04CE
6A86 051C 6A87 031D
; ; ; ;
get sub get sub
lowest byte lowest byte 2nd byte of 2nd byte of
of U1 into w of AARG, save in U1 into w AARG, save in
DS00532B-page 61
4-245

04CF 7E28 04D0 7F29 04D1 E12B 04D2 04D3 04D4 04D5 04D6 04D7 04D8 04D9 04DA 04DB 04DC 04DD 04DE 04DF 04E0 04E1 6A18 0F80 6A19 1181 6A1A 1182 6A1B 1183 6A84 0186 6A85 0187 290A 32B9 C4E9 78B9 grabloop 04E2 04E3 04E4 04E5 04E6 04E7 04E8 8804 1B80 1B81 1B82 1B83 1718 C4E2 grabok 04E9 2995 04EA 9F83 04EB C4F9 poss 04EC 04ED 04EE 04EF 04F0 04F1 04F2 04F3 04F4 04F5 04F6 04F7 04F8 04F9 04FA 04FB 04FC 04FD 04FE 04FF 0500 0501 0502 0503 0504 6A82 B580 0983 290A 3283 C505 1595 2983 B07F 4A82 2B81 2B80 C505 negs 6A82 B37F 0B83 2B0A 3083 C505 2B95 2B83 2982 8782 2981 2980 zero6bits 0505 0506 0507 0508 6A81 4A88 6A82 4A89 MOV24 MOVFP MOVPF MOVFP MOVPF Y+B1,YPWM+B0 ; Y+B1+B0,wreg ; wreg,YPWM+B0+B0 ; Y+B1+B1,wreg ; wreg,YPWM+B0+B1 ; move Y to YPWM and zero 6 bits get byte of Y+B1 into w move to YPWM+B0(B0) get byte of Y+B1 into w move to YPWM+B0(B1) movfp iorlw andwf setf cpfslt goto setf setf clrf bsf clrf clrf Y+B2,wreg 0x7F Y+B3 wreg Y+B3 zero6bits SATFLAG Y+B3 Y+B2 Y+B2,MSB Y+B1 Y+B0 ; check if Y <= -2**23 movfp andlw iorwf clrf cpfsgt goto incf clrf movlw movpf setf setf goto Y+B2,wreg 0x80 Y+B3 wreg Y+B3 zero6bits SATFLAG Y+B3 0x7F wreg,Y+B2 Y+B1 Y+B0 zero6bits clrf btfsc goto SATFLAG Y+B3,MSB negs ; ; ; ; saturate to middle 16 bits, keeping top 10 bits for pw1dcH and pw1dcL check if Y >= 2**23 RLC32 BCF RLCF RLCF RLCF RLCF decfsz goto Y _carry Y+B0 Y+B1 Y+B2 Y+B3 TMP grabloop MOVFP16 MOVFP MOVFP call ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC MOV16 MOVFP MOVWF MOVFP MOVWF clrf cpfsgt goto movfp KV,BARG KV+B0,BARG+B0 KV+B1,BARG+B1 Dmult DPX,Y DPX+B0,wreg Y+B0 DPX+B1,wreg Y+B1 DPX+B2,wreg Y+B2 DPX+B3,wreg Y+B3 U0,U1 U0+B0,wreg U1+B0 U0+B1,wreg U1+B1 wreg SHIFTNUM grabok SHIFTNUM,TMP ; move KV(B0) to BARG(B0) ; move KV(B1) to BARG(B1) ; ; ; ; ; ; ; ; ; ; ; ; ; ; Y=KP*U0+KI*INTEGRAL+KV*(U0-U1) get lowest byte of DPX into w add lowest byte of Y, save in Y(B0) get 2nd byte of DPX into w add 2nd byte of Y, save in Y(B1) get 3rd byte of DPX into w add 3rd byte of Y, save in Y(B2) get 4th byte of DPX into w add 4th byte of Y, save in Y(B3) push errors into U(k-1) get byte of U0 into w move to U1(B0) get byte of U0 into w move to U1(B1)
; if not, zero 6 bits ; if so, set Y=0x007FFFFF ; clear for debug purposes
; if not, zero 6 bits ; if so, set Y = 0xFF800000
DS00532B-page 62
4-246

0509 6A83 050A 4A8A doTorque 050B 050C 050D 050E B0C0 0B88 9F89 C516 tplimit 050F 9692 0510 C51C 0511 0512 0513 0514 0515 2988 2989 298A 298B C51C tmlimit 0516 9592 0517 C51C 0518 0519 051A 051B 051C 051D 051E 051F 0520 0521 0522 0523 0524 0525 0526 0527 0528 0529 052A 2988 2989 298A 298B mplimitok B07F 4A19 B0C0 4A18 6A18 0F88 6A19 1189 2919 B040 4A18 6A18 0F88 6A19 1189 testmax 052B 291A 052C 298A 052D 298B 052E 788E 052F 798F 0530 0531 0532 0533 0534 0535 0536 0537 0538 0539 053A 053B 053C 6A88 0518 6A89 0319 6A8A 031A 971A C53D 6A8E 0188 6A8F 0189 C54E testmin 053D 291A 053E 298A 053F 298B clrf clrf clrf TMP+B2 YPWM+B2 YPWM+B3 ;check pwm minimum limit clrf clrf clrf MOVFP16 MOVFP MOVFP SUB24 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB btfss goto MOV16 MOVFP MOVWF MOVFP MOVWF goto TMP+B2 ; check pwm maximum limit YPWM+B2 ; LMD18200 must have a minimum pulse YPWM+B3 ; so duty cycle must not be 0 or 100% YPWMAX,TMP YPWMAX+B0,TMP+B0; move YPWMAX(B0) to TMP(B0) YPWMAX+B1,TMP+B1; move YPWMAX(B1) to TMP(B1) YPWM,TMP YPWM+B0,wreg ; get lowest byte of YPWM into w TMP+B0 ; sub lowest byte of TMP, save in TMP(B0) YPWM+B1,wreg ; get 2nd byte of YPWM into w TMP+B1 ; sub 2nd byte of TMP, save in TMP(B1) YPWM+B2,wreg ; get 3rd byte of YPWM into w TMP+B2 ; sub 3rd byte of TMP, save in TMP(B2) TMP+B2,MSB testmin YPWMAX,YPWM ; saturate to max YPWMAX+B0,wreg ; get byte of YPWMAX into w YPWM+B0 ; move to YPWM(B0) YPWMAX+B1,wreg ; get byte of YPWMAX into w YPWM+B1 ; move to YPWM(B1) limitok movlw movpf movlw movpf ADD16 MOVFP ADDWF MOVFP ADDWFC clrf movlw movpf ADD16 MOVFP ADDWF MOVFP ADDWFC PW1DCH_INIT wreg,TMP+B1 PW1DCL_INIT wreg,TMP+B0 TMP,YPWM TMP+B0,wreg YPWM+B0 TMP+B1,wreg YPWM+B1 TMP+B1 0x40 wreg,TMP+B0 TMP,YPWM TMP+B0,wreg YPWM+B0 TMP+B1,wreg YPWM+B1 ; adjustment from bipolar to unipolar ; for 50% duty cycle btfss goto CLR32 CLRF CLRF CLRF CLRF EXTSTAT,bit5 mplimitok YPWM YPWM+B0 YPWM+B1 YPWM+B2 YPWM+B3 btfss goto CLR32 CLRF CLRF CLRF CLRF goto EXTSTAT,bit6 mplimitok YPWM YPWM+B0 YPWM+B1 YPWM+B2 YPWM+B3 mplimitok movlw andwf btfsc goto MOVFP MOVPF Y+B1+B2,wreg ; get byte of Y+B1 into w wreg,YPWM+B0+B2 ; move to YPWM+B0(B2) ; entry point for torque mode 0xC0 YPWM+B0 YPWM+B1,MSB tmlimit
4
3
; ; ; ; ; ; get lowest byte of TMP into w add lowest byte of YPWM, save in YPWM(B0) get 2nd byte of TMP into w add 2nd byte of YPWM, save in YPWM(B1) correct by 1 LSB add one to bit5 of pw1dcL
; ; ; ;
get add get add
lowest byte lowest byte 2nd byte of 2nd byte of
of TMP into w of YPWM, save in YPWM(B0) TMP into w YPWM, save in YPWM(B1)
DS00532B-page 63
4-247

0540 788C 0541 798D 0542 0543 0544 0545 0546 0547 0548 0549 054A 054B 054C 054D 6A88 0518 6A89 0319 6A8A 031A 9F1A C54E 6A8C 0188 6A8D 0189 limitok 054E B803 054F 7088 0550 7289 0551 0002 movlb movfp movfp bank3 ; set new duty cycle YPWM+B0,pw1dcl YPWM+B1,pw1dch MOVFP16 MOVFP MOVFP SUB24 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB btfsc goto MOV16 MOVFP MOVWF MOVFP MOVWF YPWMIN,TMP YPWMIN+B0,TMP+B0 ; move YPWMIN(B0) to TMP(B0) YPWMIN+B1,TMP+B1 ; move YPWMIN(B1) to TMP(B1) YPWM,TMP YPWM+B0,wreg ; get lowest byte of YPWM into TMP+B0 ; sub lowest byte of TMP, save YPWM+B1,wreg ; get 2nd byte of YPWM into w TMP+B1 ; sub 2nd byte of TMP, save in YPWM+B2,wreg ; get 3rd byte of YPWM into w TMP+B2 ; sub 3rd byte of TMP, save in TMP+B2,MSB limitok YPWMIN,YPWM ; saturate to min YPWMIN+B0,wreg ; get byte of YPWMIN into w YPWM+B0 ; move to YPWM(B0) YPWMIN+B1,wreg ; get byte of YPWMIN into w YPWM+B1 ; move to YPWM(B1)
w in TMP(B0) TMP(B1) TMP(B2)
return ;************************************************************** ;*************************************************************** ; NAME: doIntegral ; ; DESCRIPTION: Evaluates the integral for the servo calculations. ; doIntegral ADD16 U0,INTEGRAL U0+B0,wreg INTEGRAL+B0 U0+B1,wreg INTEGRAL+B1 ; do integral ; ; ; ; get add get add lowest byte lowest byte 2nd byte of 2nd byte of of U0 into w of INTEGRAL, save in U0 into w INTEGRAL, save in
0552 0553 0554 0555
6A84 0F96 6A85 1197 return
MOVFP ADDWF MOVFP ADDWFC
0556 0002
;**************************************************************** endif if _SERIAL_IO include serial.asm ; Serial I/O Routines ;****************************************************************** ; ; Serial I/O & Utility Functions ; ;****************************************************************
;****************************************************************** ; NAME: IdleFunction ; DESCRIPTION: ; ; IdleFunction 0557 0004 0558 0002 CLRWDT return ;**************************************************************** ;**************************************************************** ; NAME: DoCommand This routine will perform work while doing waits in serial I/O functions.
DS00532B-page 64
4-248

; ; DESCRIPTION: ; DoCommand 0559 055A 055B 055C B059 4A0D B007 4A0E movlw movpf movlw movpf tablrd tryNextCmd 055E A93A 055F A23B 0560 A93C 0561 6A3A 0562 30C1 0563 C56E 0564 3139 0565 C55E 0566 E679 0567 633B 0568 623C 0569 0000 cmdFinish 056A E679 056B B00D 056C E679 056D C124 call movlw call goto PutChar CR PutChar PollingLoop ; send response character from ; command routine followed by CR tablrd tlrd tablrd movfp cpfslt goto cpfseq goto call movfp movfp NOP 0,1,CMDTEMP 1,CMDPTRH 0,1,CMDPTRL CMDTEMP,wreg ZERO noCommand CMDCHAR tryNextCmd PutChar CMDPTRH,pclath CMDPTRL,pcl ; echo command ; indirect jump to command routine ; error if end of table ; read entry from table (CMD_TABLE & 0xff) wreg,tblptrl page CMD_TABLE wreg,tblptrh 1,1,CMDTEMP ; CMD_TABLE LSB ; CMD_TABLE MSB Search command table for command and execute it.
055D AB3A
4
3
056E B03F 056F C56A
noCommand movlw CMD_BAD goto 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 0570 B021 0571 C56A movlw goto CMD_OK cmdFinish
;************************************************************************** ;************************************************************************** ; NAME: do_move ; DESCRIPTION: ; ; ; ; ; ; ; ; 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.
DS00532B-page 65
4-249

; ; ARGUMENTS: ; do_move if 0572 E6CC call else call endif 0573 9F93 0574 C57E 0575 0576 0577 0578 0579 057A 057B 057C 057D 6A31 4A5B 6A32 4A5C 6A33 4A5D 8793 B021 C56A 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 ; ; ; ; ; ; ; ; if so, accept value into NMOVVAL get byte of VALBUF into w move to NMOVVAL(B0) get byte of VALBUF into w move to NMOVVAL(B1) get byte of VALBUF into w move to NMOVVAL(B2) set buffer full flag GetVal M [800000,7FFFFF]
DECIO GetDecVal
057E B03F 057F C56A
CMD_BAD ; else, return error cmdFinish
;************************************************************************** ;************************************************************************** ; 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 0580 0581 0582 0583 0584 0585 0586 0587 0588 0589 058A 058B 058C 058D 058E 058F 0590 0591 0592 0593 E557 E681 31C2 C580 E676 4A4D 2991 testP B050 314D C58B C598 testV B056 314D C590 1591 C598 testT B054 314D C596 2B91 movlw cpfseq goto setf T STRVALL modeerror MODETYPE ; TORQUE Moves for type T movlw cpfseq goto incf goto V STRVALL testT MODETYPE modeok ; velocity moves for type V movlw cpfseq goto goto P STRVALL testV modeok ; position moves for type P call call cpfseq goto call movpf clrf IdleFunction GetChk ONE do_mode GetChar wreg,STRVALL MODETYPE ; get single character loop
; MODETYPE=0 for position moves
; MODETYPE=1 for velocity moves
; MODETYPE=-1 for torque moves
DS00532B-page 66
4-250

0594 2990 0595 C598 0596 B03F 0597 C56A 0598 6A4D 0599 E679 059A B021 059B C56A clrf goto modeerror movlw goto modeok movfp call movlw goto SERVOFLAG modeok CMD_BAD cmdFinish STRVALL,wreg PutChar CMD_OK cmdFinish ; 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] ; ; KP=proportional gain 2 [8000,7FFF] ; KP=velocity gain 3 [8000,7FFF] ; KP=integral gain 4 [8000,7FFF] ; ; IM=integrator mode 5 [0,3] ; ; FV=velocity FF 6 [8000,7FFF] : Not Imple ; FA=acceleration FF 7 [8000,7FFF] : Not Imple ; ; ; ARGUMENTS: S [0,FF] [800000,7FFFFF] ; do_setparameter 059C E669 059D B008 059E 3031 059F C5C1 05A0 05A1 05A2 05A3 B07C 4A0D B007 4A0E call movlw cpfslt goto movlw movpf movlw movpf tablrd setNextPar 05A5 A23D 05A6 A93E 05A7 A93F 05A8 B008 05A9 303D 05AA C5C1 05AB 6A3D 05AC 3131 05AD C5A5 05AE 6A3F 05AF 690A tlrd tablrd tablrd movlw cpfslt goto movfp cpfseq goto movfp movfp 1,PARTEMP 0,1,PARLEN 0,1,PARPTR NUMPAR PARTEMP Serror PARTEMP,wreg VALBUF+B0 setNextPar PARPTR,wreg wreg,fsr1 ; pointer to parameter in fsr1 ; read entry from table GetPar NUMPAR VALBUF+B0 Serror (PAR_TABLE & 0xff) wreg,tblptrl page PAR_TABLE wreg,tblptrh 1,1,PARTEMP ; get parameter number ; check if in range [0,NUMPAR]
4
3
; PAR_TABLE LSB ; PAR_TABLE MSB
05A4 AB3D
; error if end of table
DS00532B-page 67
4-251

if 05B0 E6CC call else call endif 05B1 B031 05B2 610A movlw VALBUF movfp wreg,fsr0 AUTOINC BSF BCF BSF BCF _fs0 _fs1 _fs2 _fs3 ; pointer to VALBUF in fsr0 ; set autoincrement GetVal DECIO GetDecVal ; get new value in VALBUF
05B3 05B4 05B5 05B6
8404 8D04 8604 8F04
05B7 05B8 05B9 05BA
6800 073E 333E C5B7
setGetMore movfp decf tstfsz goto AUTONO
indf0,indf1 PARLEN PARLEN setGetMore
; move new value to parameter
; no autoincrement _fs0 _fs1 _fs2 _fs3 CMD_OK cmdFinish
05BB 05BC 05BD 05BE
8404 8504 8604 8704
BSF BSF BSF BSF movlw goto Serror
05BF B021 05C0 C56A
05C1 B03F 05C2 C56A
movlw goto
CMD_BAD cmdFinish
;************************************************************************** ;************************************************************************** ; NAME: do_readparameter ; ; DESCRIPTION: Returns the present value of a parameter. ; ; ARGUMENTS: R [0,FF] ; ; RETURNS: The present value of the requested parameter is returned. do_readparameter 05C3 E669 05C4 B008 05C5 3031 05C6 C5EB 05C7 05C8 05C9 05CA B07C 4A0D B007 4A0E call movlw cpfslt goto movlw movpf movlw movpf tablrd readNextPar 05CC A23D tlrd 1,PARTEMP ; read entry from table GetPar NUMPAR VALBUF+B0 Rerror (PAR_TABLE & 0xff) wreg,tblptrl page PAR_TABLE wreg,tblptrh 1,1,PARTEMP ; get parameter number ; check if in range [0,NUMPAR]
; PAR_TABLE LSB ; PAR_TABLE MSB
05CB AB3D
DS00532B-page 68
4-252

05CD A93E 05CE A93F 05CF B008 05D0 303D 05D1 C5EB 05D2 6A3D 05D3 3131 05D4 C5CC 05D5 6A3F 05D6 690A 05D7 B031 05D8 610A tablrd tablrd movlw cpfslt goto movfp cpfseq goto movfp movfp 0,1,PARLEN 0,1,PARPTR NUMPAR PARTEMP Rerror PARTEMP,wreg VALBUF+B0 readNextPar PARPTR,wreg wreg,fsr1 ; pointer to parameter in fsr1 ; error if end of table
movlw VALBUF movfp wreg,fsr0 AUTOINC BSF BCF BSF BCF CLR24 _fs0 _fs1 _fs2 _fs3 VALBUF VALBUF+B0 VALBUF+B1 VALBUF+B2
; pointer to VALBUF in fsr1 ; set autoincrement
05D9 05DA 05DB 05DC
8404 8D04 8604 8F04
; clear old VALBUF
4
3
05DD 2931 05DE 2932 05DF 2933
CLRF CLRF CLRF
05E0 05E1 05E2 05E3
6008 073E 333E C5E0
readGetMore movfp decf tstfsz goto AUTONO
indf1,indf0 PARLEN PARLEN readGetMore
; read parameter into VALBUF
; no autoincrement BSF BSF BSF BSF if _fs0 _fs1 _fs2 _fs3 DECIO PutDecVal ; send parameter value
05E4 05E5 05E6 05E7
8404 8504 8604 8704
05E8 E728
call else call endif
PutVal
05E9 B021 05EA C56A Rerror 05EB B03F 05EC C56A
movlw goto
CMD_OK cmdFinish
movlw goto
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.
DS00532B-page 69
4-253

; ; ; ARGUMENTS: ; ; RETURNS: ; do_shutter MOV24 05ED 05EE 05EF 05F0 05F1 05F2 6A55 4A40 6A56 4A41 6A57 4A42 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF POSITION,CPOSITION POSITION+B0,wreg wreg,CPOSITION+B0 POSITION+B1,wreg wreg,CPOSITION+B1 POSITION+B2,wreg wreg,CPOSITION+B2 ; capture commanded position ; ; ; ; ; ; get byte of POSITION into w move to CPOSITION(B0) get byte of POSITION into w move to CPOSITION(B1) get byte of POSITION into w move to CPOSITION(B2)
C The time since the start of the present move is returned.
MOV24 05F3 05F4 05F5 05F6 05F7 05F8 6A58 4A43 6A59 4A44 6A5A 4A45 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF
VELOCITY,CVELOCITY VELOCITY+B0,wreg wreg,CVELOCITY+B0 VELOCITY+B1,wreg wreg,CVELOCITY+B1 VELOCITY+B2,wreg wreg,CVELOCITY+B2
; capture commanded velocity ; ; ; ; ; ; get byte of VELOCITY into w move to CVELOCITY(B0) get byte of VELOCITY into w move to CVELOCITY(B1) get byte of VELOCITY into w move to CVELOCITY(B2)
MOV24 05F9 05FA 05FB 05FC 05FD 05FE 6A72 4A46 6A73 4A47 6A74 4A48 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF
MPOSITION,CMPOSITION MPOSITION+B0,wreg wreg,CMPOSITION+B0 MPOSITION+B1,wreg wreg,CMPOSITION+B1 MPOSITION+B2,wreg wreg,CMPOSITION+B2
; capture measured position ; ; ; ; ; ; get byte of MPOSITION into w move to CMPOSITION(B0) get byte of MPOSITION into w move to CMPOSITION(B1) get byte of MPOSITION into w move to CMPOSITION(B2)
MOV24 05FF 0600 0601 0602 0603 0604 6A75 4A49 6A76 4A4A 6A77 4A4B MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF clrf MOV16 MOVFP MOVWF MOVFP MOVWF if 060A E728 call else call endif 060B B021 060C C56A movlw goto
MVELOCITY,CMVELOCITY MVELOCITY+B0,wreg wreg,CMVELOCITY+B0 MVELOCITY+B1,wreg wreg,CMVELOCITY+B1 MVELOCITY+B2,wreg wreg,CMVELOCITY+B2 VALBUF+B2 MOVTIME,VALBUF MOVTIME+B0,wreg VALBUF+B0 MOVTIME+B1,wreg VALBUF+B1 DECIO PutDecVal
; capture measured velocity ; ; ; ; ; ; get byte of MVELOCITY into w move to CMVELOCITY(B0) get byte of MVELOCITY into w move to CMVELOCITY(B1) get byte of MVELOCITY into w move to CMVELOCITY(B2)
0605 2933
; capture move time, move to VALBUF ; ; ; ; get byte of MOVTIME into w move to VALBUF(B0) get byte of MOVTIME into w move to VALBUF(B1)
0606 0607 0608 0609
6A67 0131 6A68 0132
PutVal
CMD_OK cmdFinish
DS00532B-page 70
4-254

;***************************************************************************** ;***************************************************************************** ; 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 060D 060E 060F 0610 0611 0612 6A40 4A31 6A41 4A32 6A42 4A33 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF if 0613 E728 call else call endif 0614 B021 0615 C56A movlw goto CMD_OK cmdFinish PutVal CPOSITION,VALBUF CPOSITION+B0,wreg wreg,VALBUF+B0 CPOSITION+B1,wreg wreg,VALBUF+B1 CPOSITION+B2,wreg wreg,VALBUF+B2 DECIO PutDecVal ; move CPOSITION to VALBUF ; ; ; ; ; ; get byte of CPOSITION into w move to VALBUF(B0) get byte of CPOSITION into w move to VALBUF(B1) get byte of CPOSITION into w move to VALBUF(B2)
4
3
;*************************************************************************** ;*************************************************************************** ; 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 0616 0617 0618 0619 061A 061B 6A43 4A31 6A44 4A32 6A45 4A33 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF if 061C E728 call else call PutVal CVELOCITY,VALBUF CVELOCITY+B0,wreg wreg,VALBUF+B0 CVELOCITY+B1,wreg wreg,VALBUF+B1 CVELOCITY+B2,wreg wreg,VALBUF+B2 DECIO PutDecVal ; move commanded velocity to VALBUF ; ; ; ; ; ; get byte of CVELOCITY into w move to VALBUF(B0) get byte of CVELOCITY into w move to VALBUF(B1) get byte of CVELOCITY into w move to VALBUF(B2)
DS00532B-page 71
4-255

endif 061D B021 061E C56A 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 061F 0620 0621 0622 0623 0624 6A46 4A31 6A47 4A32 6A48 4A33 CMPOSITION,VALBUF MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF if 0625 E728 call else call endif 0626 B021 0627 C56A movlw goto CMD_OK cmdFinish PutVal CMPOSITION+B0,wreg wreg,VALBUF+B0 CMPOSITION+B1,wreg wreg,VALBUF+B1 CMPOSITION+B2,wreg wreg,VALBUF+B2 DECIO PutDecVal ; move measured position to ; ; ; ; ; ; get byte of CMPOSITION into w move to VALBUF(B0) get byte of CMPOSITION into w move to VALBUF(B1) get byte of CMPOSITION into w move to VALBUF(B2)
;************************************************************************** ;************************************************************************** ; NAME: do_readactvelocity ; ; DESCRIPTION: Returns the measured velocity multiplied by 256 which was ; captured during the last shutter command. ; ; ARGUMENTS: v ; ; RETURNS: The last captured measured velocity times 256 is returned. ; [800000,7FFFFF] ; do_readactvelocity MOV24 0628 0629 062A 062B 062C 062D 6A49 4A31 6A4A 4A32 6A4B 4A33 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 ; ; ; ; ; ; get byte of CMVELOCITY into w move to VALBUF(B0) get byte of CMVELOCITY into w move to VALBUF(B1) get byte of CMVELOCITY into w move to VALBUF(B2)
DS00532B-page 72
4-256

if 062E E728 call else call endif 062F B021 0630 C56A movlw goto CMD_OK cmdFinish PutVal DECIO PutDecVal
;*************************************************************************** ;*************************************************************************** ; 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 ; ; RETURNS: The external status register is returned. ; do_externalstatus 0631 0632 0633 0634 0635 8406 6A92 2992 8C06 E688 bsf movfp clrf bcf call movlw goto _glintd EXTSTAT,wreg EXTSTAT _glintd PutHex CMD_OK cmdFinish
4
3
0636 B021 0637 C56A
;*************************************************************************** ;*************************************************************************** ; 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 0638 6A93 0639 E688 063A B021 063B C56A movfp call movlw goto MOVSTAT,wreg PutHex CMD_OK cmdFinish
DS00532B-page 73
4-257

;*************************************************************************** ;*************************************************************************** ; NAME: do_readindposition ; ; DESCRIPTION: Returns the last index position captured in position counts. ; ; ARGUMENTS: I ; ; RETURNS: The last captured index position is returned. ; do_readindposition MOV24 063C 063D 063E 063F 0640 0641 6AB6 4A31 6AB7 4A32 6AB8 4A33 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF if 0642 E728 call else call endif 0643 B021 0644 C56A movlw goto CMD_OK cmdFinish PutVal INDEXPOS,VALBUF INDEXPOS+B0,wreg wreg,VALBUF+B0 INDEXPOS+B1,wreg wreg,VALBUF+B1 INDEXPOS+B2,wreg wreg,VALBUF+B2 DECIO PutDecVal ; move measured velocity to VALBUF ; ; ; ; ; ; get byte of INDEXPOS into w move to VALBUF(B0) get byte of INDEXPOS into w move to VALBUF(B1) get byte of INDEXPOS into w move to VALBUF(B2)
;*************************************************************************** ;*************************************************************************** ; 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 ; ; ARGUMENTS: H [800000,7FFFFF] ; do_setposition if 0645 E6CC call else call endif MOV24 0646 0647 0648 0649 064A 064B 6A31 4A55 6A32 4A56 6A33 4A57 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 ; ; ; ; ; ; get byte of VALBUF into w move to POSITION(B0) get byte of VALBUF into w move to POSITION(B1) get byte of VALBUF into w move to POSITION(B2) GetVal DECIO GetDecVal
DS00532B-page 74
4-258

MOV24 064C 064D 064E 064F 0650 0651 6A31 4A72 6A32 4A73 6A33 4A74 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CLR32 0652 0653 0654 0655 2980 2981 2982 2983 CLRF CLRF CLRF CLRF movlw goto VALBUF,MPOSITION VALBUF+B0,wreg wreg,MPOSITION+B0 VALBUF+B1,wreg wreg,MPOSITION+B1 VALBUF+B2,wreg wreg,MPOSITION+B2 Y Y+B0 Y+B1 Y+B2 Y+B3 CMD_OK cmdFinish ; ; ; ; ; ; get byte of VALBUF into w move to MPOSITION(B0) get byte of VALBUF into w move to MPOSITION(B1) get byte of VALBUF into w move to MPOSITION(B2)
0656 B021 0657 C56A
;*************************************************************************** ;*************************************************************************** ; NAME: do_reset ; ; DESCRIPTION: Performs a software reset. ; ; ARGUMENTS: Z ; do_reset 0658 B021 0659 E679 065A C021 movlw call goto CMD_OK PutChar Startup
4
3
;*************************************************************************** ; NAME: do_stop ; ; DESCRIPTION: Stops servo by clearing SERVOFLAG. ; do_stop 065B 2990 065C B021 065D C56A clrf movlw goto SERVOFLAG CMD_OK cmdFinish
;*************************************************************************** ;*************************************************************************** ; NAME: do_capture ; do_capture if 065E E6CC endif if ((_PICMASTER_DEBUG == 1) && (DECIO == 0)) call endif if _PICMASTER_DEBUG == 1 GetVal ((_PICMASTER_DEBUG == 1) && (DECIO == 1)) call GetDecVal
DS00532B-page 75
4-259

MOV16 065F 0660 0661 0662 6A31 01BC 6A32 01BD MOVFP MOVWF MOVFP MOVWF MOV16 0663 0664 0665 0666 6A31 01BE 6A32 01BF MOVFP MOVWF MOVFP MOVWF movlw goto endif ;*************************************************************************** ; NAME: GetPar ; ; DESCRIPTION: Get a parameter number [0,FF] from the serial port and place ; it in VALBUF+B0. ; GetPar CLR24 0669 2931 066A 2932 066B 2933 066C 066D 066E 066F 0670 E6B2 6A4E B50F 4A31 1D31 CLRF CLRF CLRF call movfp andlw movpf swapf call movfp addwf movpf return ;************************************************************************** ;*************************************************************************** ; NAME: GetChar ; ; DESCRIPTION: Get character from receive buffer. ; GetChar 0676 B800 0677 540A 0678 0002 movlb movpf return ;*************************************************************************** ;*************************************************************************** ; NAME: PutChar ; ; DESCRIPTION: send character out the serial port ; bank0 rcreg,wreg ; set bank0 ; receive character VALBUF VALBUF+B0 VALBUF+B1 VALBUF+B2 GetHex HEXVAL,wreg 0x0F wreg,VALBUF+B0 VALBUF+B0 GetHex VALBUF+B0,wreg HEXVAL,W wreg,VALBUF+B0 VALBUF,CAPCOUNT VALBUF+B0,wreg CAPCOUNT+B0 VALBUF+B1,wreg CAPCOUNT+B1 VALBUF,CAPTMP VALBUF+B0,wreg CAPTMP+B0 VALBUF+B1,wreg CAPTMP+B1 CMD_OK cmdFinish ; ; ; ; get byte of VALBUF into w move to CAPTMP(B0) get byte of VALBUF into w move to CAPTMP(B1) ; ; ; ; get byte of VALBUF into w move to CAPCOUNT(B0) get byte of VALBUF into w move to CAPCOUNT(B1)
0667 B021 0668 C56A
0671 E6B2 0672 6A31 0673 0E4E 0674 4A31 0675 0002
DS00532B-page 76
4-260

; ARGUMENTS: ; PutChar 0679 B801 bufwait 067A 9116 067B C67A 067C B800 shfwait 067D 9115 067E C67D 067F 4A16 0680 0002 btfss goto movpf return ;*************************************************************************** ;*************************************************************************** ; NAME: GetChk ; ; DESCRIPTION: Check if character is in receive buffer. ; GetChk 0681 0682 0683 0684 B801 560A B501 0002 movlb movpf andlw return bank1 pir,wreg CHARREADY ; set bank1 ; return status in wreg _trmt shfwait wreg,txreg ; is transmit shift register empty? btfss goto movlb _tbmt bufwait bank0 ; set bank0 movlb bank1 ; set bank1 ; is transmit buffer empty? wreg contains byte to be transmitted
; if so, send character
4
3
;*************************************************************************** ;*************************************************************************** ; 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 0685 B130 0686 E679 0687 0002 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 0x30 PutChar ; convert to ASCII DECIO
DS00532B-page 77
4-261

0688 0689 068A 068B 068C 068D 068E 068F 0690 0691 0692 4A4E 1D0A B50F 4A4F 2D0A B109 970A C693 B037 0E4F C695 puth20 0693 B030 0694 0E4F puth25 0695 E679 0696 0697 0698 0699 069A 069B 069C 069D 069E 069F 6A4E B50F 4A4F 2D0A B109 970A C6A0 B037 0E4F C6A2 putl20 06A0 B030 06A1 0E4F putl25 06A2 E679 06A3 0002 call return ;*************************************************************************** ;*************************************************************************** ; NAME: PutStr ; ; DESCRIPTION: Sends a character string out the serial port. ; PutStr 06A4 AB4C tablrd GetNextPair tlrd tablrd movfp cpfseq goto return putH 06AB E679 06AC 06AD 06AE 06AF 6A4D 31C1 C6B0 0002 putL 06B0 E679 06B1 C6A5 call goto PutChar GetNextPair call movfp cpfseq goto return PutChar STRVALL,wreg ZERO putL 1,1,STRVALH PutChar movlw addwf 0 HEXTMP,W call movfp andlw movpf negw addlw btfss goto movlw addwf goto PutChar HEXVAL,wreg 0x0F wreg,HEXTMP wreg 0x09 wreg,MSB putl20 A-0x0A HEXTMP,W putl25 movlw addwf 0 HEXTMP,W movpf swapf andlw movpf negw addlw btfss goto movlw addwf goto wreg,HEXVAL wreg 0x0F wreg,HEXTMP wreg 0x09 wreg,MSB puth20 A-0x0A HEXTMP,W puth25
06A5 A24C 06A6 A94D 06A7 06A8 06A9 06AA 6A4C 31C1 C6AB 0002
1,STRVALH 0,1,STRVALL STRVALH,wreg ZERO putH
;************************************************************************** ;***************************************************************************
DS00532B-page 78
4-262

; NAME: ; ; DESCRIPTION: ; ; ; RETURNS: GetHex getnxt 06B2 06B3 06B4 06B5 06B6 06B7 06B8 06B9 06BA 06BB 06BC 06BD 06BE E557 E681 31C2 C6B2 2950 E676 4A4E E679 B00D 044E 330A C6BF C6C9 gth10 06BF 06C0 06C1 06C2 6A4E B239 970A C6C5 movfp sublw btfss goto movlw addwf gth20 06C5 06C6 06C7 06C8 B00F 0B4E 2950 0002 gthCR 06C9 B001 06CA 4A50 06CB 0002 movlw movpf return 0x01 wreg,HEXSTAT movlw andwf clrf return 0x0F HEXVAL HEXSTAT HEXVAL,wreg 9 wreg,MSB gth20 0x09 HEXVAL call call cpfseq goto clrf call movpf call movlw subwf tstfsz goto goto IdleFunction GetChk ONE getnxt HEXSTAT GetChar wreg,HEXVAL PutChar CR HEXVAL,W wreg gth10 gthCR GetHex Receive an ASCII hex character from the serial port and convert to numerical value. numerical value in HEXVAL
4
3
06C3 B009 06C4 0F4E
;***************************************************************************
;*************************************************************************** ; 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 movfp VALBUF+B2 VALBUF+B2,wreg GetHex 0x01 HEXSTAT shift VALBUF
DS00532B-page 79
4-263

andlw movpf swapf movfp andlw addwf movfp andlw movpf swapf movfp andlw addwf movfp andlw addwf movpf goto endif ;*************************************************************************** ;*************************************************************************** ; 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 06CC 2931 06CD 2932 06CE 2933 06CF 06D0 06D1 06D2 06D3 E708 2B9B B001 3199 299B CLRF CLRF CLRF call setf movlw cpfseq clrf getdecnext call movlw cpfseq goto goto mul10 RLC24 06D9 06DA 06DB 06DC 8804 1B31 1B32 1B33 BCF RLCF RLCF RLCF MOV24 06DD 06DE 06DF 06E0 6A31 4A34 6A32 4A35 MOVFP MOVPF MOVFP MOVPF VALBUF _carry VALBUF+B0 VALBUF+B1 VALBUF+B2 VALBUF,DVALBUF VALBUF+B0,wreg wreg,DVALBUF+B0 VALBUF+B1,wreg wreg,DVALBUF+B1 ; save in DVALBUF ; ; ; ; get byte of VALBUF into w move to DVALBUF(B0) get byte of VALBUF into w move to DVALBUF(B1) ; multiply VALBUF by two DECIO 0xF0 wreg,VALBUF+B2 VALBUF+B1 VALBUF+B1,wreg 0x0F VALBUF+B2 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
VALBUF VALBUF+B0 VALBUF+B1 VALBUF+B2 GetDec DECSIGN DEC_MN DECSTAT DECSIGN
06D4 E708 06D5 06D6 06D7 06D8 B002 3199 C6D9 C6FD
GetDec DEC_CR DECSTAT mul10 fixsign
DS00532B-page 80
4-264

06E1 6A33 06E2 4A36 MOVFP MOVPF RLC24 06E3 06E4 06E5 06E6 8804 1B31 1B32 1B33 BCF RLCF RLCF RLCF RLC24 06E7 06E8 06E9 06EA 8804 1B31 1B32 1B33 BCF RLCF RLCF RLCF ADD24 06EB 06EC 06ED 06EE 06EF 06F0 6A34 0F31 6A35 1132 6A36 1133 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC CLR24 06F1 06F2 06F3 06F4 06F5 06F6 06F7 06F8 06F9 06FA 06FB 2934 2935 2936 6A98 4A34 6A34 0F31 6A35 1132 6A36 1133 CLRF CLRF CLRF movfp movpf ADD24 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC goto fixsign 06FD 290A 06FE 329B 06FF 0002 clrf cpfsgt return NEG24 COMF COMF COMF CLRF INCF ADDWFC ADDWFC return endif wreg DECSIGN VALBUF VALBUF+B0 VALBUF+B1 VALBUF+B2 wreg VALBUF+B0 VALBUF+B1 VALBUF+B2 VALBUF+B2,wreg wreg,DVALBUF+B2 VALBUF _carry VALBUF+B0 VALBUF+B1 VALBUF+B2 VALBUF _carry VALBUF+B0 VALBUF+B1 VALBUF+B2 DVALBUF,VALBUF DVALBUF+B0,wreg VALBUF+B0 DVALBUF+B1,wreg VALBUF+B1 DVALBUF+B2,wreg VALBUF+B2 DVALBUF DVALBUF+B0 DVALBUF+B1 DVALBUF+B2 DECVAL,wreg wreg,DVALBUF+B0 DVALBUF,VALBUF DVALBUF+B0,wreg ; VALBUF+B0 ; DVALBUF+B1,wreg ; VALBUF+B1 ; DVALBUF+B2,wreg ; VALBUF+B2 ; getdecnext ; 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 DVALBUF into w of VALBUF, save in DVALBUF into w VALBUF, save in DVALBUF into w VALBUF, save in ; VALBUF now multiplied by eight ; get byte of VALBUF into w ; move to DVALBUF(B2)
4
3
get add get add get add
lowest byte lowest byte 2nd byte of 2nd byte of 3rd byte of 3rd byte of
of DVALBUF into of VALBUF, save DVALBUF into w VALBUF, save in DVALBUF into w VALBUF, save in
w in VALBUF(B1) VALBUF(B2)
06FC C6D4
0700 0701 0702 0703 0704 0705 0706
1331 1332 1333 290A 1531 1132 1133
0707 0002
DS00532B-page 81
4-265

;*************************************************************************** ;*************************************************************************** ; 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 goto movlw subwf cpfslt goto gtd09 071B 071C 071D 071E B00F 0B98 2999 0002 gtdCR 071F B002 0720 4A99 0721 0002 gtdMN 0722 B001 0723 4A99 0724 0002 gtdSP 0725 B000 0726 4A99 0727 0002 movlw movpf return endif ;*************************************************************************** DEC_SP wreg,DECSTAT movlw movpf return DEC_MN wreg,DECSTAT movlw movpf return DEC_CR wreg,DECSTAT movlw andwf clrf return 0x0F DECVAL DECSTAT DECIO
0708 0709 070A 070B
E557 E681 31C2 C708
IdleFunction GetChk ONE getdecnxt GetChar wreg,DECVAL PutChar CR DECVAL,W ZERO gtdCR MN DECVAL,W ZERO gtdMN SP DECVAL,W ZERO gtdSP
070C E676 070D 4A98 070E E679 070F 0710 0711 0712 0713 0714 0715 0716 0717 0718 0719 071A B00D 0498 30C1 C71F B02D 0498 30C1 C722 B020 0498 30C1 C725
;*************************************************************************** ; NAME: PutVal ; ; DESCRIPTION: Sends the value in VALBUF [800000,7FFFFF] out the serial ; if else PutVal DECIO
DS00532B-page 82
4-266

movfp call movfp call movfp call return endif ;*************************************************************************** ;*************************************************************************** ; NAME: PutDecVal ; ; DESCRIPTION: Send the value in VALBUF [-8388608,8388607] out the serial ; if PutDecVal 0728 9733 0729 C734 btfss goto NEG24 COMF COMF COMF CLRF INCF ADDWFC ADDWFC VALBUF+B2,MSB pdpos VALBUF VALBUF+B0 VALBUF+B1 VALBUF+B2 wreg VALBUF+B0 VALBUF+B1 VALBUF+B2 DECIO VALBUF+B2,wreg PutHex VALBUF+B1,wreg PutHex VALBUF+B0,wreg PutHex
4
3
072A 072B 072C 072D 072E 072F 0730
1331 1332 1333 290A 1531 1132 1133
0731 B02D 0732 E679 0733 C736 pdpos 0734 B020 0735 E679 pddigits 0736 0737 0738 0739 B08D 4A0D B007 4A0E
movlw call goto movlw call
MN PutChar pddigits SP PutChar
movlw movpf movlw movpf tablrd readNextDec tlrd tablrd tablrd setf cpfseq goto goto getdigit incf setf inc incf SUB24 MOVFP SUBWF
(DEC_TABLE & 0xff) wreg,tblptrl page DEC_TABLE wreg,tblptrh 0,1,DVALBUF+B0
; DEC_TABLE LSB ; DEC_TABLE MSB
073A A934
073B A034 073C AB35 073D A936 073E 073F 0740 0741 2B0A 3134 C742 C756
0,DVALBUF+B0 1,1,DVALBUF+B1 0,1,DVALBUF+B2 wreg DVALBUF+B0 getdigit unitsposition DVALBUF+B0 DECVAL DECVAL DVALBUF,VALBUF DVALBUF+B0,wreg VALBUF+B0
; read entry from table
; unitsposition if end of table
0742 1534 0743 2B98 0744 1598
; restore to power of 10 ; set DECVAL to -1 ; increment DECVAL ; check if in range ; get lowest byte of DVALBUF into w ; sub lowest byte of VALBUF, save in
0745 6A34 0746 0531
DS00532B-page 83
4-267

0747 0748 0749 074A 6A35 0332 6A36 0333 MOVFP SUBWFB MOVFP SUBWFB btfss goto ADD24 074D 074E 074F 0750 0751 0752 6A34 0F31 6A35 1132 6A36 1133 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC movfp call goto unitsposition movfp call return DVALBUF+B1,wreg ; VALBUF+B1 ; DVALBUF+B2,wreg ; VALBUF+B2 ; VALBUF+B2,MSB inc DVALBUF,VALBUF ; if so, correct VALBUF for next digit DVALBUF+B0,wreg ; get lowest byte of DVALBUF into w VALBUF+B0 ; add lowest byte of VALBUF, save in DVALBUF+B1,wreg ; get 2nd byte of DVALBUF into w VALBUF+B1 ; add 2nd byte of VALBUF, save in VALBUF(B1) DVALBUF+B2,wreg ; get 3rd byte of DVALBUF into w VALBUF+B2 ; add 3rd byte of VALBUF, save in VALBUF(B2) DECVAL,wreg PutDec readNextDec ; send DECVAL get sub get sub 2nd 2nd 3rd 3rd byte byte byte byte of of of of DVALBUF VALBUF, DVALBUF VALBUF, into save into save w in VALBUF(B1) w in VALBUF(B2)
074B 9733 074C C744
0753 6A98 0754 E685 0755 C73B
; get next table entry
0756 6A31 0757 E685 0758 0002
VALBUF+B0,wreg ; units position value now in VALBUF PutDec
endif ;***************************************************************************
;*************************************************************************** ; ; ; TABLES:
CMD_START CMD_TABLE CMD_TABLE CMD_DEF do_null,DO_NULL 0759 000D 075A 0570 DATA DATA DO_NULL do_null
CMD_DEF do_move,DO_MOVE 075B 004D 075C 0572 DATA DATA DO_MOVE do_move
CMD_DEF do_mode,DO_MODE 075D 004F 075E 0580 DATA DATA DO_MODE do_mode
CMD_DEF do_setparameter,DO_SETPARAMETER 075F 0053 0760 059C DATA DATA DO_SETPARAMETER do_setparameter
CMD_DEF do_readparameter,DO_READPARAMETER 0761 0052 0762 05C3 DATA DATA DO_READPARAMETER do_readparameter
DS00532B-page 84
4-268

CMD_DEF do_shutter,DO_SHUTTER 0763 0043 0764 05ED DATA DATA DO_SHUTTER do_shutter
CMD_DEF do_readcomposition,DO_READCOMPOSITION 0765 0050 0766 060D DATA DATA DO_READCOMPOSITION do_readcomposition
CMD_DEF do_readcomvelocity,DO_READCOMVELOCITY 0767 0056 0768 0616 DATA DATA DO_READCOMVELOCITY do_readcomvelocity
CMD_DEF do_readactposition,DO_READACTPOSITION 0769 0070 076A 061F DATA DATA DO_READACTPOSITION do_readactposition
CMD_DEF do_readactvelocity,DO_READACTVELOCITY 076B 0076 076C 0628 DATA DATA DO_READACTVELOCITY do_readactvelocity
4
3
CMD_DEF do_externalstatus,DO_EXTERNALSTATUS 076D 0058 076E 0631 DATA DATA DO_EXTERNALSTATUS do_externalstatus
CMD_DEF do_movestatus,DO_MOVESTATUS 076F 0059 0770 0638 DATA DATA DO_MOVESTATUS do_movestatus
CMD_DEF do_readindposition,DO_READINDPOSITION 0771 0049 0772 063C DATA DATA DO_READINDPOSITION do_readindposition
CMD_DEF do_setposition,DO_SETPOSITION 0773 0048 0774 0645 DATA DATA DO_SETPOSITION do_setposition
CMD_DEF do_reset,DO_RESET 0775 005A 0776 0658 DATA DATA DO_RESET do_reset
CMD_DEF do_stop,DO_STOP 0777 0073 0778 065B if DATA DATA DO_STOP do_stop
_PICMASTER_DEBUG CMD_DEF do_capture,DO_CAPTURE DATA DATA DO_CAPTURE do_capture
0779 0063 077A 065E endif
CMD_END ; 077B 0000 DATA 0x00 ;
; ; In PAR_TABLE, the code word is as follows :
DS00532B-page 85
4-269

; ; 077C 077D 077E 077F 0780 0781 0782 0783 0784 0785 0786 0787 0788 0789 078A 078B 078C 0003 0020 0103 0023 0202 0026 0302 0028 0402 002A 0501 002C 0602 002D 0702 002F 0008 Low Byte is # of bytes, Hi Byte is function code
PAR_TABLE
DATA DATA DATA DATA DATA DATA DATA DATA DATA DATA DATA DATA DATA DATA DATA DATA DATA DECIO DATA DATA DATA DATA DATA DATA DATA DATA DATA DATA DATA DATA DATA
0x0003 VL 0x0103 AL 0x0202 KP 0x0302 KV 0x0402 KI 0x0501 IM 0x0602 FV 0x0702 FA NUMPAR
if 078D 078E 078F 0790 0791 0792 0793 0794 0795 0796 0797 0798 0799 423F 000F 869F 0001 270F 0000 03E7 0000 0063 0000 0009 0000 FFFF DEC_TABLE
0x423F 0x000F 0x869F 0x0001 0x270F 0x0000 0x03E7 0x0000 0x0063 0x0000 0x0009 0x0000 0xFFFF
endif
endif if _PICMASTER_DEBUG include picmastr.asm ; PIC-MASTER Debug (data capture) File 0x8000 ; addr for dummy Table Writes (to TRACE
0060
#define CaptureAddr
079A B000 079B 010D 079C B080
;*************************************************************************** ; 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) ; ; ; doCaptureRegs ; !end! hdr !skip start! movlw (CaptureAddr & 0xff) movwf tblptrl movlw CaptureAddr/256
DS00532B-page 86
4-270

079D 010E 079E AC79 079F A67A of POSERROR capPerr 07A0 AC79 POSERROR 07A1 AC7C 07A2 A67D capVerr 07A3 AC7C 07A4 AC72 07A5 A673 capMpos 07A6 AC72 07A7 AC55 07A8 A656 capPos 07A9 AC55 07AA AC75 07AB A676 capMvel 07AC AC75 07AD AC58 07AE A659 capVel 07AF AC58 07B0 AC88 07B1 A689 capPwm 07B2 AC88 tablwt DEC16 07B3 290A 07B4 07BE 07B5 03BF CLRF DECF SUBWFB TFSZ16 07B6 6ABE 07B7 08BF 07B8 330A 07B9 0002 07BA 0001 MOVFP IORWF TSTFSZ return DATA HaltTrace clrf MOV16 0x0001 ; HALT instruction (avail only in 0,0,YPWM+B0 CAPTMP wreg CAPTMP+B0 CAPTMP+B1 CAPTMP CAPTMP+B0,wreg CAPTMP+B1,W wreg tablwt tablwt tlwt 0,0,VELOCITY+B0 0,0,YPWM+B0 1,YPWM+B1 tablwt tablwt tlwt 0,0,MVELOCITY+B0 0,0,VELOCITY+B0 1,VELOCITY+B1 tablwt tablwt tlwt 0,0,POSITION+B0 0,0,MVELOCITY+B0 1,MVELOCITY+B1 tablwt tablwt tlwt 0,0,MPOSITION+B0 0,0,POSITION+B0 1,POSITION+B1 tablwt tablwt tlwt 0,0,VELERROR+B0 0,0,MPOSITION+B0 1,MPOSITION+B1 tablwt 0,0,POSERROR+B0 ; perform actual table write of movwf tablwt tlwt tblptrh 0,0,POSERROR+B0 1,POSERROR+B1 ; setup table pointer address ; dummy tablwt ; now table latch = 16 bits contents
tablwt tlwt
0,0,VELERROR+B0 1,VELERROR+B1
; capture Velocity error
; capture measured position
; capture commanded position
4
; capture measured velocity
; capture commanded velocity
; capture commanded velocity
07BB 29BB
CAPFLAG CAPCOUNT,CAPTMP
07BC 07BD 07BE 07BF 07C0
6ABC 01BE 6ABD 01BF 0002
MOVFP CAPCOUNT+B0,wreg ; get byte of CAPCOUNT into w MOVWF CAPTMP+B0 ; move to CAPTMP(B0) MOVFP CAPCOUNT+B1,wreg ; get byte of CAPCOUNT into w MOVWF CAPTMP+B1 ; move to CAPTMP(B1) return ;************************************************************************* end END
Errors : Warnings :
0 0
DS00532B-page 87
4-271

NOTES:
DS00532B-page 88
4-272
WORLDWIDE SALES & SERVICE

AMERICAS
Corporate Ofce Microchip Technology Inc. 2355 West Chandler Blvd. Chandler, AZ 85224-6199 Tel: 602 786-7200 Fax: 602 786-7277 Technical Support: 602 786-7627 Web: http://www.mchip.com/microhip Atlanta Microchip Technology Inc. 500 Sugar Mill Road, Suite 200B Atlanta, GA 30350 Tel: 770 640-0034 Fax: 770 640-0307 Boston Microchip Technology Inc. 5 Mount Royal Avenue Marlborough, MA 01752 Tel: 508 480-9990 Fax: 508 480-8575 Chicago Microchip Technology Inc. 333 Pierce Road, Suite 180 Itasca, IL 60143 Tel: 708 285-0071 Fax: 708 285-0075 Dallas Microchip Technology Inc. 14651 Dallas Parkway, Suite 816 Dallas, TX 75240-8809 Tel: 214 991-7177 Fax: 214 991-8588 Dayton Microchip Technology Inc. 35 Rockridge Road Englewood, OH 45322 Tel: 513 832-2543 Fax: 513 832-2841 Los Angeles Microchip Technology Inc. 18201 Von Karman, Suite 455 Irvine, CA 92715 Tel: 714 263-1888 Fax: 714 263-1338 New York Microchip Technology Inc. 150 Motor Parkway, Suite 416 Hauppauge, NY 11788 Tel: 516 273-5305 Fax: 516 273-5335
AMERICAS (continued)
San Jose Microchip Technology Inc. 2107 North First Street, Suite 590 San Jose, CA 95131 Tel: 408 436-7950 Fax: 408 436-7955
EUROPE
United Kingdom Arizona Microchip Technology Ltd. Unit 6, The Courtyard Meadow Bank, Furlong Road Bourne End, Buckinghamshire SL8 5AJ Tel: 44 0 1628 851077 Fax: 44 0 1628 850259 France Arizona Microchip Technology SARL 2 Rue du Buisson aux Fraises 91300 Massy - France Tel: 33 1 69 53 63 20 Fax: 33 1 69 30 90 79 Germany Arizona Microchip Technology GmbH Gustav-Heinemann-Ring 125 D-81739 Muenchen, Germany Tel: 49 89 627 144 0 Fax: 49 89 627 144 44 Italy Arizona Microchip Technology SRL Centro Direzionale Colleoni Palazzo Pegaso Ingresso No. 2 Via Paracelso 23, 20041 Agrate Brianza (MI) Italy Tel: 39 039 689 9939 Fax: 39 039 689 9883
ASIA/PACIFIC
Hong Kong Microchip Technology Unit No. 3002-3004, Tower 1 Metroplaza 223 Hing Fong Road Kwai Fong, N.T. Hong Kong Tel: 852 2 401 1200 Fax: 852 2 401 3431 Korea Microchip Technology 168-1, Youngbo Bldg. 3 Floor Samsung-Dong, Kangnam-Ku, Seoul, Korea Tel: 82 2 554 7200 Fax: 82 2 558 5934 Singapore Microchip Technology 200 Middle Road #10-03 Prime Centre Singapore 188980 Tel: 65 334 8870 Fax: 65 334 8850 Taiwan Microchip Technology 10F-1C 207 Tung Hua North Road Taipei, Taiwan, ROC Tel: 886 2 717 7175 Fax: 886 2 545 0139
JAPAN
Microchip Technology Intl. Inc. Benex S-1 6F 3-18-20, Shin Yokohama Kohoku-Ku, Yokohama Kanagawa 222 Japan Tel: 81 45 471 6166 Fax: 81 45 471 6122 9/22/95
All rights reserved. 1995, Microchip Technology Incorporated, USA.

Information contained in this publication regarding device applications and the like is intended through suggestion only and may be superseded by updates. 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 Microchips 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. The Microchip logo and name are registered trademarks of Microchip Technology Inc. All rights reserved. All other trademarks mentioned herein are the property of their respective companies.

Electronica - Servo Control of A Dc-Brush Motor An532

Uploaded by

Copyright:

Available Formats

Electronica - Servo Control of A Dc-Brush Motor An532

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Electronica - Servo Control of A Dc-Brush Motor An532

Uploaded by

Copyright:

Available Formats

Servo Control of a DC-Brush Motor

FIGURE 1 - A TYPICAL SERVO SYSTEM

1993 Microchip Technology Inc.

Servo Control of a DC-Brush Motor

Serial Command Non-Servo I/O

FIGURE 3 - THE PIC17C42 BASED SERVO CONTROL BOARD

Place Servo Control Board photo here.

FIGURE 4 - DIGITAL PID IMPLEMENTATION

Note: The Z-1 operator indicates a one sample time delay

1993 Microchip Technology Inc.

Servo Control of a DC-Brush Motor

1993 Microchip Technology Inc.

Servo Control of a DC-Brush Motor

down_count 4x mode up_count

FIGURE 6 - ENCODER INTERFACE SCHEME

1993 Microchip Technology Inc.

Servo Control of a DC-Brush Motor

Timer 1 <8> PWM output 15.625 KHz

PWM1 PWM2 Reset Timer 2 <8>

Comparator Servo-update interrupt 0.9765625 KHz

Period-reg 2 Period=16 PIC17C42

16 PWM cycles = 1.024ms

1 PWM cycle = 64s Servo-update interrupt

FIGURE 8 - VELOCITY RAMP SEGMENTS FOR POSITION MOVES

Velocity Limit Short Move Slope = Accel. limit Long Move

1993 Microchip Technology Inc.

Servo Control of a DC-Brush Motor

doMposMvel Read up_count and down_count Measure current velocity, position

doExtStat Monitor ext status inputs

doError Computer position and velocity error

Execute Command Function

End of list? Yes Send ERROR message Send response message

1993 Microchip Technology Inc.

Servo Control of a DC-Brush Motor

TABLE 2 - EXTERNAL STATUS REGISTER BITS

TABLE 3 - MOVE STATUS REGISTER BITS

1993 Microchip Technology Inc.

Servo Control of a DC-Brush Motor

FIGURE 11 - TYPICAL SERVO RESPONSE: DESIRED/ACTUAL POSITION

2150 2100 2050 2000 0 50

Kp = 2048 Kd = 20480 Ki = 1024 Actual Desired

Capture Servo-Response: c (#Count)

0 -5 -10 -15 -20 -25

Kp = 2048 Kd = 20480 Ki = 1024

OPTIMIZING THE SYSTEM

Kp = 2048 Kd = 20480 Ki = 1024 Actual Desired

0 -2 -4 -6 -8 Kp = 2048 Kd = 20480 Ki = 1024 20 40 60 80 100 120 140 160

1993 Microchip Technology Inc.

Servo Control of a DC-Brush Motor

TABLE 4 - SERVO CONTROL CHIP COMPARISON

1993 Microchip Technology Inc.

APPENDIX A: SCHEMATIC DIAGRAM

DSR RTS +10V DTR Tx Rx CTS RI C6 +1 10E16R 2 +5V

Servo Control of a DC-Brush Motor

EEPROM 256 x 8 CS0 TEST 7 CS1 MODE[SPI] SCL[OUT] SCL[IN]

CAPTURE 6 4 OC ExtCAP 5 Int. Ext. CAP1 P5

PIC17C42 Based DC Servo Control System

Sheet 1 of 3 Controller Logic

1993 Microchip Technology Inc.