Servo Control DC-Brush Motor Author: Bucella Teknic, Inc. FI
|
|
|
Top Searches for this datasheet
AN532
Servo Control DC-Brush Motor
Author: Bucella Teknic, Inc.
FIGURE
Digital Command
TYPICAL SERVO SYSTEM
Motor Processor Power Amplifier Encoder
INTRODUCTION
PIC17C42 microcontroller excellent choice cost-effective servo control embedded applications. Harvard architecture RISC features, PIC17C42 offers excellent computation speed needed real-time closed loop servo control. This application note examines PIC17C42 brush motor servo controller. shown that (Proportional, Integral, Differential) control calculation performed less than (@16 MHz) allowing control loop sample times range. Encoder rates easily handled PIC17C42's high speed peripherals. Further, on-chip peripherals allow absolute minimum cost system constructed. Closed-loop servo motor control usually handled 16-bit, high-end microcontrollers external logic. attempt increase performance many applications upgrading DSPs (Digital Signal Processors). However, very high performance PIC17C42 makes possible implement these servo control applications significant reduction overall system cost. servo system discussed this application note uses PIC17C42 microcontroller, programmable logic device (PLD), single-chip H-bridge driver. Such system might used positioning controller printer, plotter, scanner. cost implementing servo control system using PIC17C42 allows this system compete favorably with stepper motor systems offering number advantages: Increased Acceleration, Velocity Improved Efficiency Reduced Audible Noise True Disturbance Rejection
Encoder Counter
PIC17C42 implements both servo compensator algorithm trajectory profile (trapezoidal) generation. trajectory generation algorithm necessary optimum motion implementation important servo compensator itself. servo compensator implemented traditional digital filter, fuzzy logic algorithm, simple algorithm implemented this application note). combination servo compensator trajectory calculations place significant demands processor. conversion handled conventional using PIC17C42's pulse-width modulation (PWM). either case output signal power stage which translates analog signal(s) into usable voltages currents drive motor. output duty-cycle signal combination with direction signal single signal which carries both pieces information. latter case duty cycle commands null output, duty cycle commands maximum negative output, 100% maximum positive output. amplifier configured supply controlled voltage current motor. Most embedded systems voltage output because simpler cheaper. Sequential encoders produce quadrature pulse trains, from which position, speed, direction motor rotation derived. frequency proportional speed each transition represents increment position. phase signals used determine direction rotation. These encoder signals usually decoded into Count Count Down pulses, using small state machine. These pulses then routed N-bit, up/down counter whose value corresponds position motor shaft. decoder/counter implemented hardware, software, combination two.
SYSTEM OVERVIEW
Servo Control
Modern digital servo systems formed shown Figure These systems control motor with incremental feedback device known sequential encoder. They consist encoder counter, processor, some form (Digital-to-Analog) converter, power amplifier which delivers current voltage motor.
1997 Microchip Technology Inc.
DS00532C-page
AN532
PIC17C42 Based Motor Control Board
PIC17C42 based servo system described here full RS-232 ASCII interface, on-board switching power supply, H-bridge motor drive, over-current protection, limit switch inputs digital I/O. entire system measures 3.5" shown Figure system used evaluate PIC17C42 servo applications. unused PIC17C42 pins available connector prototyping.
FIGURE
PIC17C42 BASED SERVO CONTROL BOARD
DS00532C-page
1997 Microchip Technology Inc.
AN532
algorithm used servo compensator position trajectories derived from linear velocity ramp segments. This system uses 50%-null conversion technique. power stage high current output switching stage which steps-up level signal. Encoder signal decoding accomplished using external PLD. up/down counter implemented internally PIC17C42 combination hardware software (Figure Figure
FIGURE
mode up_count down_count mode up_count down_count
SEQUENTIAL ENCODER SIGNALS
FIGURE
ENCODER INTERFACE SCHEME
TMR0 up-count RA1/T0CKI 16-bit counter 16R8 TMR3 down-count RB5/TCLK3 16-bit counter PIC17C42
1x/4x select
1997 Microchip Technology Inc.
DS00532C-page
AN532
COMPENSATOR
routine most widely used algorithm servo motor control. Although most optimum controller applications, easy understand tune. standard digital algorithm's form shown Figure U(k) position velocity error Y(k) output. This algorithm been implemented using PIC17C42's math library. Only instruction cycles required, resulting execution time MHz. Integrator windup condition which occurs controllers when large following error present system, instance when large step disturbance encountered. integrator continually builds during this following error condition even though output saturated. integrator then "unwinds" when servo system reaches final destination causing excessive oscillation. implementation shown Figure avoids this problem stopping action integrator during output saturation.
MOTOR ACTUATION
PIC17C42 contains high-resolution pulse width modulation (PWM) subsystem. This forms very efficient power converter when coupled simple switching power stage. resolution PIC17C42 subsystem 62.5 MHz). This translates into 10-bit resolution 15.6 rate part 1/2-bit) resolution kHz. This allows effective voltage control while still maintaining modulation frequency above limit human hearing. This especially relevant office automation equipment where minimizing noise design goal. motor responds output stage time averaging duty cycle output. Most motors react slowly, having electrical time constant more mechanical time constant 20.0 more. output effectively equivalent that linear amplifier. system shown Figure H-bridge's direction input wired directly PIC17C42's output. H-bridge powered supply voltage, this configuration volts presented motor when signal duty cycle, volts duty cycle volts 100% duty cycle.
FIGURE
DIGITAL IMPLEMENTATION
Optional Anti-Windup Logic
U(k)
Y(k) Saturation
Note: operator indicates sample time delay
FIGURE
PIC17C42 SERVO SYSTEM
LMD18201 Out1 PIC17C42 Out2 Brake 16R8 T0CKI TCLK3 TCLK12 16-bit timer input 16-bit timer input 8-bit timer input serial port receive transmit pins
Serial Command
PWM1 PWM2 TCLK12
Non-Servo
T0CKI TCLK3 CLKIN CLKOUT
CountUp CountDn
Osc.
DS00532C-page
1997 Microchip Technology Inc.
AN532
ENCODER FEEDBACK
Position feedback example system derived from quadrature encoder mounted motor shaft. Both incremental position direction derived from this inexpensive device. quadrature encoder signals processed 16R8-type device shown Figure converts quadrature pulses into pulse streams: Count Count Down (Figure These signals then 16-bit timers PIC17C42 (Timer3 Timer0). logic description decoder shown Appendix PIC17C42 keeps track motor shaft's incremental position differencing these 16-bit timers. This operation performed each servo sample time current position calculated adding incremental position previous position. Since both timers 16-bits, keeping track overflow unnecessary, unless encoder signals frequency greater than 32767 times sample frequency. example, servo sample time maximum encoder rate would 3.2767 MHz. Counter wraparound concern because only difference between counters used. Two's-complement subtraction takes care this automatically. Position maintained three-byte, 24-bit quantity example program shown Appendix However, there limit size internal position register. adding 16-bit incremental position each sample time N-byte software register, N-byte position maintained.
TRAJECTORY GENERATION
trajectory generation algorithm essential optimum motion control. linear piecewise velocity trajectory implemented this application. position move, velocity incremented constant acceleration value until specified maximum velocity reached. maximum velocity maintained required amount time then decremented same acceleration (deceleration) value until zero velocity attained. velocity trajectory therefore trapezoidal long move triangular short move where maximum velocity reached (Figure doPreMove subroutine invoked once beginning move calculate trajectory limits. doMove routine then invoked every sample time calculate "desired" velocity position values follows: VK-1 Acceleration) PK-1 VK-1 more details trajectory generation, Appendix
FIGURE
VELOCITY RAMP SEGMENTS POSITION MOVES Velocity
Velocity Limit Short Move
Long Move
Slope Accel. limit
Time
1997 Microchip Technology Inc.
DS00532C-page
AN532
IMPLEMENTATION DETAILS
program structure straightforward: interrupt service routine (ISR) processes servo control trajectory generation calculations, foreground loop used implement user interface, serial communication, exception processing (i.e., limit switches, watchdog timer, etc.). simple structure. order effect servo control need read encoder, calculate trajectory point values, output PWM, constant, predefined rate. initiated hardware timer (Timer2) PIC17C42. make sure that servo calculation always occurs synchronously with subsystem, PWM2 output wired input TMR12 (TMR1 internally-clocked, 8-bit timer mode; TMR2 externally-clocked, 8-bit counter mode). loaded into register. sample rate then becomes rate divided this implementation (Figure
FIGURE
SAMPLING SCHEME
TMR1x8 output 15.625
PWM1 PWM2 Reset
TMR2x8
Servo-update interrupt 0.9765625
Comparator
Period PIC17C42
cycles 1.024
cycles
cycles Servo-update interrupt
DS00532C-page
1997 Microchip Technology Inc.
AN532
FIGURE FLOWCHART FOREGROUND PROCESSING
DCMOTOR.ASM IntPoll Program setup
FIGURE
FLOWCHART INTERRUPT SERVICE ROUTINE
Save Registers doMposMvel Read up_count down_count Measure current velocity, position
IdleFunction
GetChk
doExtStat Monitor external status inputs
char?
Move Running?
Move?
doPreMove
Command
list?
Trajectory Progress?
doMove
Next Command
Execute Command Function
doError Computer position velocity error
list? Send ERROR message Send response message
Servo
doServo Compute Compute value Write value doCaptureRegs PICMASTER based debug Output position, velocity, etc. info
CAPFLAG?
Restore Registers
RETURN
1997 Microchip Technology Inc.
DS00532C-page
AN532
following events must occur interrupt service routine: Read Timers (TMR0 TMR3) Calculate Reference Position using Trajectory Generation Routine. Calculate Error: U(k) Reference Position Current Position Calculate Y(k) using output Manage other housekeeping tasks (i.e. service serial characters) entire requires only 0.250 execute with processor clock frequency. Read Parameter: [00h FFh] Returns present value parameter. Shutter: Returns time sample time counts 65,53610) since start present move captures commanded actual values position velocity time command. Read commanded position: Returns commanded position count which captured during last Shutter command. Range: -8,388,60810 8,388,60710. Read commanded velocity: Returns commanded velocity multiplied which captured during last Shutter command. Range: -8,388,60810 8,388,60710. Read actual position: Returns actual position count which captured during last Shutter command. Range: -8,388,60810 8,388,60710. Read actual velocity: Returns actual velocity multiplied which captured during last Shutter command. Range: -8,388,60810 8,388,60710. External Status: Returns digit number which defines state bits external status register. Issuing this command will clear bits external status register unless event which still true. bits defined Table
COMMAND INTERFACE
following commands implemented recognized user interface foreground loop. Move (Value): [-8,388,60810 8,388,60710] Commands axis move position velocity. Position data relative, velocity data absolute. Position data encoder counts. Velocity data given encoder counts sample time multiplied 256. moves performed controller such that velocity acceleration limits into parameter memory will violated. move commands kept deep FIFO buffer. command buffer executed soon executing command complete. move currently executing commanded move will start immediately. Mode: (Type), [P,V, argument will cause subsequent move commands incremental position moves. argument will cause subsequent moves absolute velocity moves. argument sets "Torque mode'" where subsequent commands directly write PWM. This useful debug purposes. Parameter: (#,Value) [00h FFh, -8,388,60810 8,388,60710] Sets controller parameters value given. Parameters shown Table
TABLE EXTERNAL STATUS REGISTER BITS
index marker detected +limit reached -limit reached input true
TABLE PARAMETERS
Move Status: Parameter Velocity Limit Acceleration Limit Proportional Gain Differential Gain Integral Gain Range 8,388,60710 8,388,60710 -3276810 3276710 -3276810 3276710 -3276810 3276710 Returns two-digit number which defines state bits move status register. Issuing this command will clear bits move status register unless event which still true. bits defined Table
TABLE MOVE STATUS REGISTER BITS
move buffer empty move complete
(counts sample time multiplied 256) (counts sample time sample time multiplied 256)
DS00532C-page
1997 Microchip Technology Inc.
AN532
Read Index position:
Returns last index position captured position counts. Position (Value): [-8,388,60810to8,388,60710] Sets actual commanded positions value given. Should sent unless move FIFO buffer empty. Reset: Performs software reset.
FIGURE
2250 2200 Position 2150 2100 2050 2000
TYPICAL SERVO RESPONSE
Position Response
Desired/Actual Position
Capture Servo-Response: (#Count)
command will flag inside indicating that starting with next (servo move) command, velocity position information will sent invoking doCaptureRegs procedure) during every servo-loop #count times. #count, processor will halt (see doCaptureRegs procedure). This useful debug purposes.
2048 20480 1024 Actual Desired Time (ms)
Position Error
Position Error
Disable Servo:
This command disables servo actuation. servo will activate again with execution next (move) command. This useful debug purposes. Examples:
;Reset software <CR> required) ;Set velocity servo mode ;(No <CR> required) 1000<CR> ;Set velocity 1000 M-1000<CR> ;Set velocity 1000 reverse ;direction
Error
Time (ms) 2048 20480 1024
OPTIMIZING SYSTEM
Once loop successfully implemented, next challenge tune This made simple through extensive PICMASTERIn-Circuit Emulator PIC17C42. PICMASTER highly sophisticated real-time in-circuit emulator with unlimited break-point capability, deep trace buffer external logic probes. user interface software runs under Windows® with pull-down menus on-line help. PICMASTER software also supports dynamic data exchange (DDE). makes possible send trace buffer information spreadsheet, such EXCEL®, also running under Windows. tune PID, first small amount diagnostics code added servo routine (doCaptureRegs). This code simply outputs, every sample point, actual desired position values, actual desired velocity values, position error velocity error using TABLWT instruction. These captured trace buffer emulator. 'trace' condition only trace data cycles 2-cycle TABLWT instructions. Next, trace buffer transferred EXCEL various parameters plotted. plots graphically show amounts overshoot, ripple response time. altering plotting results, system fine tuned.
Desired/Actual Velocity
Velocity Time (ms) Velocity counts/samples Velocity Response 2048 20480 1024 Actual Desired
Velocity Error
Velocity Response Velocity 2048 20480 1024 Time (ms) Velocity counts/samples
1997 Microchip Technology Inc.
DS00532C-page
AN532
Under Windows multi-tasking environment, using PICMASTER emulator, this done real time described below. Three sessions under Windows: terminal emulator session send commands motor control board. "terminal" program provided with Windows used, although communications software such PROCOMM will work. Second, PICMASTER emulation session invoked. actual PIC17C42 replaced in-circuit emulator probe. Within emulator, trace points setup capture actual desired position velocity values appropriate cycles. Third, session EXCEL started dynamically linked PICMASTER sessions such that whenever trace buffer full, data sent over EXCEL. simple filtering commands EXCEL used separate various data types, i.e. actual position data from desired position from actual velocity etc. Next, various plot windows within EXCEL plot these information.
CONCLUSIONS
Using high-performance 8-bit microcontroller heart servo control system cost-effective solution which requires very external components. comparison with popular dedicated servo-control chip, presented Table
TABLE SERVO CONTROL CHIP COMPARISON
LM629 Encoder Rate Servo Update Time PIC17C42 PIC17C42 0.25 0.16
Sampling Frequency
Once these setups have been done, every servo move, responses automatically plotted. then simple matter varying coefficients observing responses achieve desired system response. point, responses stored files and/or printed out. Except very long "move" commands, most position velocity commands executed (i.e. system settled) less than samples, making possible capture variables (actual desired position velocity, position errors servo output) PICMASTER's trace buffer.
Also apparent comparison table additional processing power available when using microcontroller. This processing used provide user interface, handle other I/O, etc. Alternatively, additional processing time might used improve performance compensator trajectory generation algorithms. further advantage that many embedded applications using motor control microcontroller proves complete, minimum cost solution.
Credit
This application note working demo board been developed Teknic Inc. Teknic (Rochester, N.Y.) specializes Motor Control Systems.
References
1.Thomas Bucella, "Comparing DSPs Microprocessors Motion Control Systems-Some Real World Data", PCIM conference proceedings 1990 Intertec Communications, Inc. 2.David Auslander, Cheng Tham, "Real-Time Software Control" 1990 Prentice-Hall, Inc., Englewood Cliffs, 3."DC Motors, Speed Controls, Servo Systems" Fifth Edition 1980 Electro-Craft Corporation, Hopkins,
DS00532C-page
1997 Microchip Technology Inc.
AN532
APPENDIX
SCHEMATIC DIAGRAM
1997 Microchip Technology Inc.
T0CKI
DS00532C-page
AN532
APPENDIX (CONT.): SCHEMATIC DIAGRAM
DS00532C-page
1997 Microchip Technology Inc.
AN532
APPENDIX (CONT.): SCHEMATIC DIAGRAM
1997 Microchip Technology Inc.
DS00532C-page
AN532
APPENDIX
Combination quadrature decoder input synchronizer. allows decoding decoding based pin. November 1991 MODULE QuadDivider; TITLE QuadDivider V1.0; COMMENT Device: 16R8; TYPE 16R8; INPUTS; RESET NODE[PIN2] INVERTED; NODE[PIN3]; NODE[PIN4]; NODE[PIN5]; INDX NODE[PIN6]; Feedback pins NODE[PIN12]; NODE[PIN13]; NODE[PIN14]; P90D NODE[PIN15]; CntUp NODE[PIN18]; CntDn NODE[PIN19]; NODE[PIN16]; COUNT NODE[PIN17] INVERTED; OUTPUTS; NODE[PIN12]; NODE[PIN13]; NODE[PIN14]; P90D NODE[PIN15]; CntUp NODE[PIN18]; CntDn NODE[PIN19]; NODE[PIN16]; COUNT NODE[PIN17] INVERTED; TABLE; !RESET; P90D !RESET; !RESET; P90D !RESET; CntUp COUNT CntDn COUNT !UP; COUNT !P90D +!P0D P90D +!P0D !P90D P90D P90D P90D +!P0D P90D !P90D !RESET; !P0D !P90D +!P0D P90D +!P0D P90D P90D P90D !P90D !P90D +!P0D !P90D !RESET; END; QuadDivider; This design
Phi0 Phi90
DS00532C-page
1997 Microchip Technology Inc.
AN532
APPENDIX
ALGORITHM FLOWCHART
Enter doServo
Compute Error Value
Compute proportional Kp.U0
saturated?
YPWM
saturated?
Bypass integral "Anti-wind-up"
Convert YPWM from unipolar bipolar
Compute integral Errror compute integral portion Ki.U
Limit 18200 requires that duty cycle 100%
YPWM Scale Y.2SHIFTNUM YPWM Positive overflow Negative overflow Extract YPWM from YPZ=Y <23::8> Save current error value previous error value FF800000h value 007FFFFFh YPWM YPWM
Zero lowest bits YPWM
Write YPWM high byte PW1DCH Write YPWM high byte PW1DCL
Check external position min./max. limit inputs
Return
1997 Microchip Technology Inc.
DS00532C-page
AN532
ALGORITHM CODE LISTING
NAME: doServo DESCRIPTION: Performs servo loop calculations. doServo MOV16 LOADAB CALL MVPF32 CLRF CPFSGT CALL LOADAB CALL ADD32 MVFP16 SUB16 MVFP16 CALL ADD32 CLRF CPFSGT GOTO MOVFP grabloop RLC32 DECFSZ GOTO grabok CLRF BTFSC GOTO poss MOVFP ANDLW IORWF CLRF CPFSGT GOTO INCF CLRF MOVLW MOVPF SETF SETF GOTO negs MOVFP IORLW ANDWF SETF CPFSLT GOTO SETF Y+B2,WREG 0x7F Y+B3 WREG Y+B3 zero6bits SATFLAG check -2**23 negative overflow, saturate maximum negative number Y+B2,WREG 0x80 Y+B3 WREG Y+B3 zero6bits SATFLAG Y+B3 0x7F WREG,Y+B2 Y+B1 Y+B0 zero6bits SATFLAG Y+B3,MSB negs saturate middle bits, keeping bits PW1DCH PW1DCL check 2**23 positive overflow, saturate maximum positive number not, zero bits Y=0x007FFFFF clear debug purposes POSERROR,U0 U0,KP Dmult DPX,Y WREG SATFLAG doIntegral INTEGRAL,KI Dmult DPX,Y U0,AARG U1,AARG KV,BARG Dmult DPX,Y WREG SHIFTNUM grabok SHIFTNUM,TMP grabloop save position error compute KP*U0 Y=KP*U0 previous output saturated, accumulate integrator antiwindup Basic calculation
compute KI*INTEGRAL Y=KP*U0+KI*INTEGRAL compute KV*(U0-U1)
Y=KP*U0+KI*INTEGRAL+KV*(U0-U1)
scale SHIFTNUM (2**SHIFTNUM)
Scale
not, zero bits 0xFF800000
DS00532C-page
1997 Microchip Technology Inc.
AN532
SETF CLRF CLRF CLRF zero6bits MOV24 doTorque MOVLW ANDWF BTFSC GOTO tplimit BTFSS GOTO CLR32 GOTO tmlimit BTFSS GOTO CLR32 mplimitok MOVLW MOVPF MOVLW MOVPF ADD16 CLRF MOVLW MOVPF ADD16 testmax CLRF CLRF CLRF MVFP16 SUB24 BTFSS GOTO MOV16 GOTO testmin CLRF CLRF CLRF MVFP16 SUB24 BTFSC GOTO MOV16 limitok MOVLB MOVFP MOVFP MOV16 RETURN BANK3 YPWM+B0,PW1DCL YPWM+B1,PW1DCH U0,U1 duty cycle Write values registers TMP+B2 YPWM+B2 YPWM+B3 YPWMIN,TMP YPWM,TMP TMP+B2,MSB limitok YPWMIN,YPWM check minimum limit TMP+B2 YPWM+B2 YPWM+B3 YPWMAX,TMP YPWM,TMP TMP+B2,MSB testmin YPWMAX,YPWM limitok check maximum limit LMD18200 must have minimum pulse duty cycle must 100% cycle must 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 unipolar duty cycle Convert from unnipolar bipolar EXTSTAT,BIT6 mplimitok YPWM mplimitok Y+B3 Y+B2 Y+B2,MSB Y+B1 Y+B0
Y+B1,YPWM+B0 0xC0 YPWM+B0 YPWM+B1,MSB tmlimit
move YPWM zero bits entry point torque mode
external position limits have been reached then zero output
correct bit5 PW1DCL
saturate
saturate
push errors into U(k-1)
1997 Microchip Technology Inc.
DS00532C-page
AN532
APPENDIX ENCODER INTERFACE ROUTINE
NAME: doMPosMVel DESCRIPTION: Calculates current position from UpCount DownCount doMPosMVel UpCounter first MVFP16 readUp MOVPF MOVPF CPFSEQ GOTO MOVPF CLRFM MOV16 SUB16 TMR0H,WREG TMR0L,UPCOUNT+B0 TMR0H readUp WREG,UPCOUNT+B1 VELOCITY+B0 UPCOUNT,MVELOCITY+B1 TMP+B0,MVELOCITY+B1 UPCOUNT,TMP+B0 save upcount
Skip next hasn't changed changed, re-read store clear bits below binary point compute upcount increment
DownCounter MVFP16 readDown MOVLB MOVPF MOVPF CPFSEQ GOTO MOVPF DOWNCOUNT,TMP+B0 BANK2 TMR3H,WREG TMR3L,DOWNCOUNT+B0 TMR3H readDown WREG,DOWNCOUNT+B1 save downcount ;timers Bank
Skip next hasn't changed changed, re-read store compute downcount increment
MVFP16 DOWNCOUNT+B0,TMP+B2 SUB16 TMP+B0,TMP+B2 SUB16 CLRF BTFSC SETF TMP+B2,MVELOCITY+B1 MVELOCITY+B3 MVELOCITY+B2,MSB MVELOCITY+B3
compute measured velocity sign extend measured velocity addition measured position
ADD24
MVELOCITY+B1,MPOSITION
compute measured position delta position measured velocity
RETURN
DS00532C-page
1997 Microchip Technology Inc.
AN532
APPENDIX
doPreMove
This routine executed only once beginning each move. First, various buffers flags initialized test modetype performed. position mode, minimum move triangular consists steps. Therefore, (MOVVAL) immediate move performed. Otherwise, normal move generation possible with sign move MOVSIGN appropriate signed velocity acceleration limits MOVVAL/2 HMOVVAL. velocity mode, sign move calculated MOVSIGN appropriate signed velocity acceleration limits placed Finally, modeready, MOVVAL sign extended higher precision arithmetic servo enabled. torque mode, MOVVAL output directly servo disabled, doMove executed.
IMPLEMENTATION DETAILS TRAJECTORY GENERATION
With units sample times, time increment between subsequent sample times yielding iterative equations updating position velocity implemented doPosVel given
where signed acceleration limit calculated doPreMove. inverse equations this iteration, necessary undoing unwanted step, contained undoPosVel given position mode, actual shape velocity profile depends values size move. Either velocity limit reached before half move completed, resulting trapezoidal velocity profile, half move completed before velocity limit realized, resulting triangular velocity profile. algorithm employed here, velocity limit treated bound actual velocity limit, thereby permitting exactly same number steps during speedup speed down sections move. Phase defined section move where commanded position less than half move, phase remaining portion move. time when actual velocity limit reached time phase
doMove
Move generation based piecewise constant acceleration model. During constant acceleration, this results standard equations position velocity given
FIGURE
T2+1
FIGURE
FIGURE
final velocity
initial velocity
1997 Microchip Technology Inc.
DS00532C-page
AN532
Furthermore, amount undershoot amount overshoot half move Discretization error minimized using values whether more step will reduce size final immediate move during last step move. triangular move, discretization error given min. (2x, 2y), resulting condition that then take more speedup step. case trapezoidal move, discretization error given min. (2x, yielding condition that take more step during flat section phase2. beginning doMove, MOVTIME incremented doPosVel called evaluate next proposed values commanded position velocity under current value position mode, phase1, original position plus half move minus proposed commanded position calculated placed MOVDEL, with previous MOVDEL saved MOVTMP. half move would passed, MOVTMP MOVDEL with first time indicating that phase1 about completed. Therefore, continue phase1, where maximum velocity been reached, proposed commanded position executed. other hand, proposed move would exceed maximum velocity, undo proposed move, current acceleration zero, reevaluate iterative equations with acceleration, MOVTIME execute move. Since cleared doPreMove, used flag indicate this corner velocity profile been reached. Once find that drop into code that executed only time, with phase2 beginning next step. maximum velocity been reached, velocity profile triangular. this case, negated speed down, x>y, more step needed minimize discretization error. negated, proposed step undone, again negated speed down step recalculated executed, with T1=MOVTIME zero, indicating that flat section phase1, then t2net1, where MOVTIME then more phase2 flat step necessary minimize discretization error. PH2FLAT defined number steps flat section phase2, used counter during completion. then PH2FLAT T2-T1, otherwise PH2FLAT T2-T1-1 phase1 finally complete. subsequent steps will proceed through phase2, first deciding flat section finished checking PH2FLAT reached zero. not, flat where PH2FLAT decremented, tested zero. speed down section begun calculating appropriate signed acceleration limit executing last flat section moves. following steps, PH2FLAT leaving only final test zero commanded velocity indicate move. This will always occur since actual maximum velocity, bounded above user supplied limit, always integer multiple user supplied acceleration limit, with exactly same number steps taken during speedup speed down. velocity mode much more straightforward, with velocity profile form ramp. final velocity been reached, move continues maximum acceleration. final velocity been reached, acceleration zero move generation commanded position velocity continued unless final velocity zero.
DS00532C-page
1997 Microchip Technology Inc.
1997 Microchip Technology Inc. DS00532C-page
Please check Microchip latest version source code. Microchip's Worldwide Address: www.microchip.com; Bulletin Board Support: MCHIPBBS using CompuServe® (CompuServe membership required).
APPENDIX
COMPLETE CODE LISTING (DCMOTOR.LST)
DCMOTOR.ASM 1-16-1997 13:20:16 PAGE
MPASM 01.40 Released
OBJECT CODE VALUE
LINE SOURCE TEXT
00F42400 003D0900 000003E8 0000000C 00000019 00000067 000000CF 000000FF 00000019
00001 00002 00003 00004 00005 00006 00007 00008 00009 00010 00011 00012 00013 00014 00015 00001 00002 00003 00004 00005 00006 00007 00008 00009 00010 00011 00012 00013 00014 00015 00016 00017 00018 00019 00020
TITLE "DCMOTOR SERVO CONTROL: Revision: Revised: 8/5/92 Program: DCMOTOR.ASM Revision Date: 1-13-97 Compatibility with MPASMWIN 1.40 CREDIT: Developed Teknic Inc. 1992 PROCESSOR LIST PIC17C42 17C42, COLUMNS=120, XREF=YES, NOWRAP, LINES=255, R=DEC
#include "dcmotor.h17" Header file dcmotor.asm: Revised: 8/5/92 hardware constants MASTER_CLOCK 16000000 MHz: change diff clock speed CLKOUT MASTER_CLOCK/4 SAMPLE_RATE 1000 BAUD19200 BAUD9600 BAUD2400 BAUD1200 BAUD_MIN BAUD_DEFAULT 0xFF BAUD9600
AN532
DS00532C-page 1997 Microchip Technology Inc.
AN532
00000006 0000003F 000000FF 0000000F 0000007F 000000C0 00000080 00000090 00000020 00000019 000000F3 00000000
00000040 00000001 00000080 000000FE
00000000 00000001 00000000 00000001 00000002 00000003 00000007 00000000
00000000
00021 00022 00023 00024 00025 00026 00027 00028 00029 00030 00031 00032 00033 00034 00035 00036 00037 00038 00039 00040 00041 00042 00043 00044 00045 00046 00047 00048 00049 00050 00051 00052 00053 00054 00055 00056 00057 00058 00059 00060 00061 00062 00063 00064 00065 00066 00067
TCON1_INIT TCON2_INIT PR1_INIT PR2_INIT PW1DCH_INIT PW1DCL_INIT RTCSTA_INIT RCSTA_INIT TXSTA_INIT SPBRG_INIT
0x06 0x3F 0xFF frequency CLKOUT/256 (PR1_INIT/2) duty cycle 50%, PW1DCH PR1_INIT/2 0xC0 PW1DCL 0xC0 0x80 0x90 0x20 BAUD_DEFAULT
DDRB_INIT 0xF3 DDRD_INIT 0x00 values PWMINL 0x40 PWMINH 0x01 PWMAXL 0x80 PWMAXH 0xFE 17c42 constants define special function registers: #define #define #define #define #define true false TRUE FALSE
0x0000 0x0140 (min 0xFFC0 0x0140
cblock 0x00
1997 Microchip Technology Inc. DS00532C-page
00000000
00000000 00000008
00000010
00000010
00000010
00000016 00000017
00000010
00068 00069 00070 00071 00072 00073 00074 00075 00076 00077 00078 00079 00080 00081 00082 00083 00084 00085 00086 00087 00088 00089 00090 00091 00092 00093 00094 00095 00096 00097 00098 00099 00100 00101 00102 00103 00104 00105 00106 00107 00108 00109 00110 00111 00112 00113 00114
endc cblock 0x00 define banks BANK0,BANK1,BANK2,BANK3 endc cblock 0x00 unbanked registers endc cblock 0x10 bank0 registers endc cblock 0x10 endc cblock 0x10 endc CA1L CA1H cblock endc define commonly used bits: ALUSTA definitions #define #define #define #define #define #define #define #define #define #define _carry _fs0 _fs1 _fs2 _fs3 ALUSTA,0 ALUSTA,0 ALUSTA,0 ALUSTA,1 ALUSTA,2 ALUSTA,3 ALUSTA,4 ALUSTA,5 ALUSTA,6 ALUSTA,7 0x16 0x17 0x10 alternate function bank2 registers bank1 registers
define bank3 variables
AN532
T0STA definitions
DS00532C-page 1997 Microchip Technology Inc.
AN532
00115 00116 00117 00118 00119 00120 00121 00122 00123 00124 00125 00126 00127 00128 00129 00130 00131 00132 00133 00134 00135 00136 00137 00138 00139 00140 00141 00142 00143 00144 00145 00146 00147 00148 00149 00150 00151 00152 00153 00154 00155 00156 00157 00158 00159 00160 00161
#define #define #define #define #define #define #define
_ps0 _ps1 _ps2 _ps3 _tosc _tose _intedg
T0STA,1 T0STA,2 T0STA,3 T0STA,4 T0STA,5 T0STA,6 T0STA,7
CPUSTA definitions #define #define #define #define #define _npd _nto _gint _glintd _stkav CPUSTA,2 CPUSTA,3 CPUSTA,4 CPUSTA,4 CPUSTA,5
INTSTA definitions #define #define #define #define #define #define #define #define _inte _toie _t0ckie _peie _intf _t0if _t0ckif _peif INTSTA,0 INTSTA,1 INTSTA,2 INTSTA,3 INTSTA,4 INTSTA,5 INTSTA,6 INTSTA,7
definitions #define #define #define #define #define #define #define #define _rcif _txif _ca1if _ca2if _tmr1if _tmr2if _tmr3if _rbif PIR,0 PIR,1 PIR,2 PIR,3 PIR,4 PIR,5 PIR,6 PIR,7
definitions #define #define #define #define #define _rcie _txie _ca1ie _ca2ie _tmr1ie PIE,0 PIE,1 PIE,2 PIE,3 PIE,4
1997 Microchip Technology Inc. DS00532C-page
00162 00163 00164 00165 00166 00167 00168 00169 00170 00171 00172 00173 00174 00175 00176 00177 00178 00179 00180 00181 00182 00183 00184 00185 00186 00187 00188 00189 00190 00191 00192 00193 00194 00195 00196 00197 00198 00199 00200 00201 00202 00203 00204 00205 00206 00207 00208
#define _tmr2ie PIE,5 #define _tmr3ie PIE,6 #define _rbie PIE,7 RCSTA definitions #define #define #define #define #define #define #define _rx9d _oerr _ferr _cren _cren _rx9 _spen RCVSTA,0 RCVSTA,1 RCVSTA,2 RCVSTA,4 RCVSTA,5 RCVSTA,6 RCVSTA,7
TXSTA definitions #define #define #define #define #define #define _tx9d _trmt _sync _txen _tx9 _csrc TXSTA,0 TXSTA,1 TXSTA,4 TXSTA,5 TXSTA,6 TXSTA,7
TCON1 definitions #define #define #define #define #define #define #define #define _tmr1cs _tmr2cs _tmr3cs _t16 _ca1ed0 _ca1ed1 _ca2ed0 _ca2ed1 TCON1,0 TCON1,1 TCON1,2 TCON1,3 TCON1,4 TCON1,5 TCON1,6 TCON1,7
TCON2 definitions #define #define #define #define #define _tmr1on _tmr2on _tmr3on _ca1pr3 _pwm1on TCON2,0 TCON2,1 TCON2,2 TCON2,3 TCON2,4 TCON2,5 TCON2,6 TCON2,7
AN532
#define _pwm2on #define _ca1ovf #define _ca2ovf
DS00532C-page 1997 Microchip Technology Inc.
AN532
0000000D 00000018 00000008 00000020 0000000A 0000002D
00000001
00000001
00000008
00000021 0000003F
00000000 00000001 00000002 00000003 00000000 00000001 00000002 00000003
0000000D 0000004D
00209 00210 00211 00212 00213 00214 00215 00216 00217 00218 00219 00220 00221 00222 00223 00224 00225 00226 00227 00228 00229 00230 00231 00232 00233 00234 00235 00236 00237 00238 00239 00240 00241 00242 00243 00244 00245 00246 00247 00248 00249 00250 00251 00252 00253 00254 00255
ascii constants 0x0D 0x18 0x08 0x20 0x0A DECIO TRUE true decimal, false cmds constants macros CHARREADY 0x01 NUMPAR 0x08 Response characters CMD_OK CMD_BAD Exit values HEX_SP 0x00 HEX_MN 0x01 HEX_CR 0x02 HEX_CAN 0x03 DEC_SP 0x00 DEC_MN 0x01 DEC_CR 0x02 DEC_CAN 0x03 Command characters DO_NULL DO_MOVE
1997 Microchip Technology Inc. DS00532C-page
0000004F 00000053 00000052 00000043 00000050 00000056 00000070 00000076 00000058 00000059 00000049 00000048 0000005A 00000073 00000063
00000002
00256 00257 00258 00259 00260 00261 00262 00263 00264 00265 00266 00267 00268 00269 00270 00271 00272 00273 00274 00275 00276 00277 00278 00279 00280 00281 00282 00283 00284 00285 00286 00287 00288 00289 00290 00291 00292 00293 00294 00295 00296 00297 00298 00299 00300 00301 00302
DO_MODE DO_SETPARAMETER DO_READPARAMETER DO_SHUTTER DO_READCOMPOSITION DO_READCOMVELOCITY DO_READACTPOSITION DO_READACTVELOCITY DO_EXTERNALSTATUS DO_MOVESTATUS DO_READINDPOSITION DO_SETPOSITION DO_RESET DO_STOP DO_CAPTURE
NAME: CMD_DEF DESCRIPTION: Creates definitions command table data struc; ture. first word command character used, second word pointer function that handles this command function. ENTRY CONDITIONS: Must contiguous with other entries function work. ARGUMENTS: FUNC command execution function ROOT NAME ROOT CMD_DEF MACRO DATA DATA ENDM FUNC,ROOT ROOT FUNC
CMD_ENTRY_LENGTH
AN532
NAME: CMD_START DESCRIPTION: Labels start command table.
DS00532C-page 1997 Microchip Technology Inc.
AN532
00303 00304 00305 00306 00307 00308 00309 00310 00311 00312 00313 00314 00315 00316 00317 00318 00319 00320 00321 00322 00323 00324 00325 00326 00327 00328 00329 00330 00331 00332 00333 00334 00335 00336 00337 00338 00339 00340 00341 00342 00343 00344 00345 00346 00347 00348 00349
CMD_START MACRO LABEL LABEL ENDM
NAME: CMD_END DESCRIPTION: Marks command table with entry 0x00 CMD_END MACRO DATA ENDM
0x00
NAME: CLR32 DESCRIPTION: Clear consecutive bytes data memory ARGUMENTS: TIMING (cycles): CLR32 MACRO CLRF CLRF CLRF CLRF ENDM a+B0, a+B1, a+B2, a+B3,
NAME: CLR24 DESCRIPTION: Clear consecutive bytes data memory ARGUMENTS:
1997 Microchip Technology Inc. DS00532C-page
00350 00351 00352 00353 00354 00355 00356 00357 00358 00359 00360 00361 00362 00363 00364 00365 00366 00367 00368 00369 00370 00371 00372 00373 00374 00375 00376 00377 00378 00379 00380 00381 00382 00383 00384 00385 00386 00387 00388 00389 00390 00391 00392 00393 00394 00395 00396
TIMING (cycles): CLR24 MACRO CLRF CLRF CLRF ENDM NAME: CLR16 DESCRIPTION: Clear consecutive bytes data memory ARGUMENTS: TIMING(cycles): CLR16 MACRO CLRF CLRF ENDM NAME: MOV32 DESCRIPTION: move ARGUMENTS: TIMING (cycles):8 a+B0, a+B1, a+B0, a+B1, a+B2,
AN532
MOV32 MACRO MOVFP MOVPF MOVFP MOVPF MOVFP
a+B0,WREG WREG,b+B0 a+B1,WREG WREG,b+B1 a+B2,WREG byte into move b(B0) byte into move b(B1) byte into
DS00532C-page 1997 Microchip Technology Inc.
AN532
00397 00398 00399 00400 00401 00402 00403 00404 00405 00406 00407 00408 00409 00410 00411 00412 00413 00414 00415 00416 00417 00418 00419 00420 00421 00422 00423 00424 00425 00426 00427 00428 00429 00430 00431 00432 00433 00434 00435 00436 00437 00438 00439 00440 00441 00442 00443
MOVPF MOVFP MOVPF ENDM
WREG,b+B2 a+B3,WREG WREG,b+B3
move b(B2) byte into move b(B3)
NAME: MOV24 DESCRIPTION: move ARGUMENTS: TIMING (cycles): MOV24 MACRO MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF ENDM NAME: MOV16 DESCRIPTION: move ARGUMENTS: TIMING cycles): MOV16 MACRO MOVFP MOVPF MOVFP MOVPF a+B0,WREG WREG,b+B0 a+B1,WREG WREG,b+B1 byte into move b(B0) byte into move b(B1) a+B0,WREG WREG,b+B0 a+B1,WREG WREG,b+B1 a+B2,WREG WREG,b+B2 byte into move b(B0) byte into move b(B1) byte into move b(B2)
1997 Microchip Technology Inc. DS00532C-page
00444 00445 00446 00447 00448 00449 00450 00451 00452 00453 00454 00455 00456 00457 00458 00459 00460 00461 00462 00463 00464 00465 00466 00467 00468 00469 00470 00471 00472 00473 00474 00475 00476 00477 00478 00479 00480 00481 00482 00483 00484 00485 00486 00487 00488 00489 00490
ENDM NAME: MVPF32 DESCRIPTION: move from data memory data memory ARGUMENTS: TIMING (cycles): MVPF32 MACRO MOVPF MOVPF MOVPF MOVPF ENDM NAME: MVPF24 DESCRIPTION: move from data memory data memory ARGUMENTS: TIMING (cycles): MVPF24 MACRO MOVPF MOVPF MOVPF ENDM A+B0,B+B0 A+B1,B+B1 A+B2,B+B2 move A(B0) B(B0) move A(B1) B(B1) move A(B2) B(B2) A+B0,B+B0 A+B1,B+B1 A+B2,B+B2 A+B3,B+B3 move move move move A(B0) A(B1) A(B2) A(B3) B(B0) B(B1) B(B2) B(B3)
AN532
DS00532C-page 1997 Microchip Technology Inc.
AN532
00491 00492 00493 00494 00495 00496 00497 00498 00499 00500 00501 00502 00503 00504 00505 00506 00507 00508 00509 00510 00511 00512 00513 00514 00515 00516 00517 00518 00519 00520 00521 00522 00523 00524 00525 00526 00527 00528 00529 00530 00531 00532 00533 00534 00535 00536 00537
NAME: MVPF16 DESCRIPTION: move from data memory data memory ARGUMENTS: TIMING (cycles): MVPF16 MACRO MOVPF MOVPF ENDM A+B0,B+B0 A+B1,B+B1 move A(B0) B(B0) move A(B1) B(B1)
NAME: MVFP32 DESCRIPTION: move from data memory data memory ARGUMENTS: TIMING (cycles): MVFP32 MACRO MOVFP MOVFP MOVFP MOVFP ENDM NAME: MVFP24 DESCRIPTION: move from data memory data memory ARGUMENTS: A+B0,B+B0 A+B1,B+B1 A+B2,B+B2 A+B3,B+B3 move move move move A(B0) A(B1) A(B2) A(B3) B(B0) B(B1) B(B2) B(B3)
1997 Microchip Technology Inc. DS00532C-page
00538 00539 00540 00541 00542 00543 00544 00545 00546 00547 00548 00549 00550 00551 00552 00553 00554 00555 00556 00557 00558 00559 00560 00561 00562 00563 00564 00565 00566 00567 00568 00569 00570 00571 00572 00573 00574 00575 00576 00577 00578 00579 00580 00581 00582 00583 00584
TIMING (cycles): MVFP24 MACRO MOVFP MOVFP MOVFP ENDM NAME: MVFP16 DESCRIPTION: move from data memory data memory ARGUMENTS: TIMING (cycles): MVFP16 MACRO MOVFP MOVFP ENDM NAME: LOADAB DESCRIPTION: Loads extended math library AARG BARG ARGUMENTS: AARG BARG TIMING (cycles): LOADAB MACRO MOVFP MOVFP MOVFP A+B0,AARG+B0 A+B1,AARG+B1 B+B0,BARG+B0 load byte AARG load byte AARG load byte BARG A+B0,B+B0 A+B1,B+B1 move A(B0) B(B0) move A(B1) B(B1) A+B0,B+B0 A+B1,B+B1 A+B2,B+B2 move A(B0) B(B0) move A(B1) B(B1) move A(B2) B(B2)
AN532
DS00532C-page 1997 Microchip Technology Inc.
AN532
00585 00586 00587 00588 00589 00590 00591 00592 00593 00594 00595 00596 00597 00598 00599 00600 00601 00602 00603 00604 00605 00606 00607 00608 00609 00610 00611 00612 00613 00614 00615 00616 00617 00618 00619 00620 00621 00622 00623 00624 00625 00626 00627 00628 00629 00630 00631
MOVFP ENDM
B+B1,BARG+B1
load byte BARG
NAME: ADD32 DESCRIPTION: ARGUMENTS: TIMING (cycles): ADD32 MACRO MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC ENDM NAME: ADD24 DESCRIPTION: ARGUMENTS: TIMING (cycles): ADD24 MACRO MOVFP ADDWF MOVFP ADDWFC a+B0,WREG b+B0, a+B1,WREG b+B1, lowest byte lowest byte byte byte into save b(B0) into save b(B1) a+B0,WREG b+B0, a+B1,WREG b+B1, a+B2,WREG b+B2, a+B3,WREG b+B3, lowest byte lowest byte byte byte byte byte byte byte into save b(B0) into save b(B1) into save b(B2) into save b(B3)
1997 Microchip Technology Inc. DS00532C-page
00632 00633 00634 00635 00636 00637 00638 00639 00640 00641 00642 00643 00644 00645 00646 00647 00648 00649 00650 00651 00652 00653 00654 00655 00656 00657 00658 00659 00660 00661 00662 00663 00664 00665 00666 00667 00668 00669 00670 00671 00672 00673 00674 00675 00676 00677 00678
MOVFP ADDWFC ENDM
a+B2,WREG b+B2,
byte into byte save b(B2)
NAME: ADD16 DESCRIPTION: ARGUMENTS: TIMING (cycles): ADD16 MACRO MOVFP ADDWF MOVFP ADDWFC ENDM NAME: SUB32 DESCRIPTION: subtract ARGUMENTS: TIMING (cycles): a+B0,WREG b+B0, a+B1,WREG b+B1, lowest byte lowest byte byte byte into save b(B0) into save b(B1)
AN532
SUB32 MACRO MOVFP SUBWF MOVFP SUBWFB MOVFP
a+B0,WREG b+B0, a+B1,WREG b+B1, a+B2,WREG lowest byte lowest byte byte byte byte into save b(B0) into save b(B1) into
DS00532C-page 1997 Microchip Technology Inc.
AN532
00679 00680 00681 00682 00683 00684 00685 00686 00687 00688 00689 00690 00691 00692 00693 00694 00695 00696 00697 00698 00699 00700 00701 00702 00703 00704 00705 00706 00707 00708 00709 00710 00711 00712 00713 00714 00715 00716 00717 00718 00719 00720 00721 00722 00723 00724 00725
SUBWFB MOVFP SUBWFB ENDM
b+B2, a+B3,WREG b+B3,
byte save b(B2) byte into byte save b(B3)
NAME: SUB24 DESCRIPTION: subtract ARGUMENTS: TIMING cycles): SUB24 MACRO MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB ENDM NAME: SUB16 DESCRIPTION: subtract ARGUMENTS: TIMING (cycles): SUB16 MACRO MOVFP SUBWF MOVFP SUBWFB a+B0,WREG b+B0, a+B1,WREG b+B1, lowest byte lowest byte byte byte into save b(B0) into save b(B1) a+B0,WREG b+B0, a+B1,WREG b+B1, a+B2,WREG b+B2, lowest byte lowest byte byte byte byte byte into save b(B0) into save b(B1) into save b(B2)
1997 Microchip Technology Inc. DS00532C-page
00726 00727 00728 00729 00730 00731 00732 00733 00734 00735 00736 00737 00738 00739 00740 00741 00742 00743 00744 00745 00746 00747 00748 00749 00750 00751 00752 00753 00754 00755 00756 00757 00758 00759 00760 00761 00762 00763 00764 00765 00766 00767 00768 00769 00770 00771 00772
ENDM NAME: RLC32 DESCRIPTION: rotate left ARGUMENTS: TIMING (cycles): RLC32 MACRO RLCF RLCF RLCF RLCF ENDM NAME: RLC24 DESCRIPTION: rotate left ARGUMENTS: TIMING (cycles): RLC24 MACRO RLCF RLCF RLCF ENDM _carry a+B0, a+B1, a+B2, _carry a+B0, a+B1, a+B2, a+B3,
AN532
DS00532C-page 1997 Microchip Technology Inc.
AN532
00773 00774 00775 00776 00777 00778 00779 00780 00781 00782 00783 00784 00785 00786 00787 00788 00789 00790 00791 00792 00793 00794 00795 00796 00797 00798 00799 00800 00801 00802 00803 00804 00805 00806 00807 00808 00809 00810 00811 00812 00813 00814 00815 00816 00817 00818 00819
NAME: RLC16 DESCRIPTION: rotate left ARGUMENTS: TIMING (cycles): RLC16 MACRO RLCF RLCF ENDM NAME: RRC32 DESCRIPTION: rotate right ARGUMENTS: TIMING (cycles): RRC32 MACRO RLCF RRCF RRCF RRCF RRCF ENDM NAME: RRC24 DESCRIPTION: rotate right a+B3,W a+B3, a+B2, a+B1, a+B0, move sign into carry _carry a+B0, a+B1,
1997 Microchip Technology Inc. DS00532C-page
00820 00821 00822 00823 00824 00825 00826 00827 00828 00829 00830 00831 00832 00833 00834 00835 00836 00837 00838 00839 00840 00841 00842 00843 00844 00845 00846 00847 00848 00849 00850 00851 00852 00853 00854 00855 00856 00857 00858 00859 00860 00861 00862 00863 00864 00865 00866
ARGUMENTS: TIMING (cycles): RRC24 MACRO RLCF RRCF RRCF RRCF ENDM NAME: RRC16 DESCRIPTION: rotate right ENTRY CONDITIONS: TIMING (cycles): RRC16 MACRO RLCF RRCF RRCF ENDM NAME: INC24 DESCRIPTION: increment ARGUMENTS: TIMING (cycles): a+B1,W a+B1, a+B0, move sign into carry a+B2,W a+B2, a+B1, a+B0, move sign into carry
AN532
DS00532C-page 1997 Microchip Technology Inc.
AN532
00867 00868 00869 00870 00871 00872 00873 00874 00875 00876 00877 00878 00879 00880 00881 00882 00883 00884 00885 00886 00887 00888 00889 00890 00891 00892 00893 00894 00895 00896 00897 00898 00899 00900 00901 00902 00903 00904 00905 00906 00907 00908 00909 00910 00911 00912 00913
INC24 MACRO CLRF INCF ADDWFC ADDWFC ENDM
WREG, a+B0, a+B1, a+B2,
NAME: INC16 DESCRIPTION: increment ARGUMENTS: TIMING (cycles): INC16 MACRO CLRF INCF ADDWFC ENDM NAME: DEC24 DESCRIPTION: Decrement Number ARGUMENTS: TIMING (cycles): DEC24 MACRO CLRF DECF SUBWFB SUBWFB WREG, a+B0, a+B1, a+B2, WREG, a+B0, a+B1,
1997 Microchip Technology Inc. DS00532C-page
00914 00915 00916 00917 00918 00919 00920 00921 00922 00923 00924 00925 00926 00927 00928 00929 00930 00931 00932 00933 00934 00935 00936 00937 00938 00939 00940 00941 00942 00943 00944 00945 00946 00947 00948 00949 00950 00951 00952 00953 00954 00955 00956 00957 00958 00959 00960
ENDM DESCRIPTION: Decrement Number ARGUMENTS: TIMING (cycles): DEC16 MACRO CLRF DECF SUBWFB ENDM NAME: NEG32 DESCRIPTION: negate ARGUMENTS: TIMING (cycles): NEG32 MACRO COMF COMF COMF COMF CLRF INCF ADDWFC ADDWFC ADDWFC ENDM A+B0, A+B1, A+B2, A+B3, WREG, A+B0, A+B1, A+B2, A+B3, WREG, a+B0, a+B1,
AN532
DS00532C-page 1997 Microchip Technology Inc.
AN532
00961 00962 00963 00964 00965 00966 00967 00968 00969 00970 00971 00972 00973 00974 00975 00976 00977 00978 00979 00980 00981 00982 00983 00984 00985 00986 00987 00988 00989 00990 00991 00992 00993 00994 00995 00996 00997 00998 00999 01000 01001 01002 01003 01004 01005 01006 01007
NAME: NEG24 DESCRIPTION: negate ARGUMENTS: TIMING (cycles): NEG24 MACRO COMF COMF COMF CLRF INCF ADDWFC ADDWFC ENDM NAME: NEG16 DESCRIPTION: negate ARGUMENTS: TIMING (cycles): NEG16 MACRO COMF COMF CLRF INCF ADDWFC ENDM A+B0, A+B1, WREG, A+B0, A+B1, A+B0, A+B1, A+B2, WREG, A+B0, A+B1, A+B2,
1997 Microchip Technology Inc. DS00532C-page
01008 01009 01010 01011 01012 01013 01014 01015 01016 01017 01018 01019 01020 01021 01022 01023 01024 01025 01026 01027 01028 01029 01030 01031 01032 01033 01034 01035 01036 01037 01038 01039 01040 01041 01042 01043 01044 01045 01046 01047 01048 01049 01050 01051 01052 01053 01054
NAME: AUTONO DESCRIPTION: Sets auto increment decrement TIMING (cycles): AUTONO MACRO ENDM NAME: AUTOINC DESCRIPTION: auto increment TIMING (cycles): AUTOINC MACRO ENDM NAME: AUTODEC DESCRIPTION: Sets auto decrement TIMING (cycles): AUTODEC MACRO _fs0 _fs1 _fs2 _fs3 _fs0 _fs1 _fs2 _fs3
AN532
DS00532C-page 1997 Microchip Technology Inc.
AN532
01055 01056 01057 01058 01059 01060 01061 01062 01063 01064 01065 01066 01067 01068 01069 01070 01071 01072 01073 01074 01075 01076 01077 01078 01079 01080 01081 01082 01083 01084 01085 01086 01087 01088 01089 01090 01091 01092 01093 01094 01095 01096 01097 01098 01099 01100 01101
ENDM
_fs0 _fs1 _fs2 _fs3
NAME: TFSZ32 DESCRIPTION: test skip zero TIMING (cycles): TFSZ32 MACRO MOVFP IORWF IORWF IORWF TSTFSZ ENDM A+B0,WREG A+B1,W A+B2,W A+B3,W WREG
NAME: TFSZ24 DESCRIPTION: test skip zero TIMING (cycles): TFSZ24 MACRO MOVFP IORWF IORWF TSTFSZ ENDM A+B0,WREG A+B1,W A+B2,W WREG
1997 Microchip Technology Inc. 00000018 0000001C 00000018 0000001C 00000020 00000023 00000026 00000028 0000002A 0000002C 0000002D 0000002F DS00532C-page 00000031 00000034 00000037 0000003A 0000003C 00000040
01102 01103 01104 01105 01106 01107 01108 01109 01110 01111 01112 01113 01114 01115 01116 00016 00017 00018 00019 00020 00021 00022 00023 00024 00025 00026 00027 00028 00029 00030 00031 00032 00033 00034 00035 00036 00037 00038 00039 00040 00041 00042 00043 00044 00045 00046 00047
NAME: TFSZ16 DESCRIPTION: test skip zero TIMING (cycles): TFSZ16 MACRO MOVFP IORWF TSTFSZ ENDM A+B0,WREG A+B1,W WREG
global variables CBLOCK 0x18 DPX,DPX1,DPX2,DPX3 arithmetic accumulator AARG,AARG1,BARG,BARG1 multiply arguments ENDC CBLOCK 0x18 TMP,TMP1,TMP2,TMP3 MOVTMP,MOVTMP1,MOVTMP2,MOVTMP3 ENDC CBLOCK 0x20 VL,VL1,VL2 AL,AL1,AL2 KP,KP1 KV,KV1 KI,KI1 FV,FV1 FA,FA1 VALBUF,VALBUF1,VALBUF2 CVALBUF,CVALBUF1,CVALBUF2 DVALBUF,DVALBUF1,DVALBUF2 ISRBSR,ISRWREG CMDCHAR,CMDTEMP,CMDPTRH,CMDPTRL PARTEMP,PARLEN,PARPTR
temporary variables move temporary storage
velocity limit acceleration limit proportional gain velocity gain integral gain integrator mode velocity feedforward acceleration feedforward
AN532
iovalue buffer iovalue buffer iovalue buffer save storage command interface variables parameter variables
DS00532C-page 1997 Microchip Technology Inc.
AN532
00000043 00000046 00000049 0000004C 0000004F 00000051 00000054 00000056 00000058 0000005B 0000005E 00000062 00000066 0000006A 0000006C 0000006D 0000006F 00000071 00000073 00000074 00000075 00000078 0000007A 0000007C 0000007F 00000082 00000083 00000087 0000008B 0000008F 00000095 00000096 00000097 00000098 00000099 0000009A 0000009B
00048 00049 00050 00051 00052 00053 00054 00055 00056 00057 00058 00059 00060 00061 00062 00063 00064 00065 00066 00067 00068 00069 00070 00071 00072 00073 00074 00075 00076 00077 00078 00079 00080 00081 00082 00083 00084 00085 00086 00087 00088 00089 00090 00091 00092 00093 00094
CPOSITION,CPOSITION1,CPOSITION2 CVELOCITY,CVELOCITY1,CVELOCITY2 STRVALH,STRVALL HEXVAL,HEXTMP,HEXSTAT OPOSITION,OPOSITION1 OPOSITION2,OPOSITION3 POSITION,POSITION1,POSITION2 VELOCITY,VELOCITY1,VELOCITY2
shutter shutter shutter shutter
commanded position commanded velocity measured position measured velocity
string variables variables
original commanded position commanded position commanded velocity move move half move value value move value time sample counts
MOVVAL,MOVVAL1,MOVVAL2,MOVVAL3 MOVTIME,MOVTIME1 MOVSIGN T1,T11 T2,T21 TAU,TAU1 NMODE MODE MPOSITION,MPOSITION1,MPOSITION2 MVELOCITY,MVELOCITY1 MVELOCITY2,MVELOCITY3 POSERROR,POSERROR1,POSERROR2 VELERROR,VELERROR1,VELERROR2 SIGN Y,Y1,Y2,Y3 YPWM,YPWM1,YPWM2,YPWM3 YPWMIN,YPWMIN1,YPWMAX,YPWMAX1 U0,U01,U1,U11,U2,U21 SERVOFLAG MODETYPE EXTSTAT MOVSTAT MOVFLAG SATFLAG INTEGRAL,INTEGRAL1
0x00 positive, 0x80 negative time maximum velocity time half move total move time next move modetype move modetype
measured position measured velocity position error velocity error multiply sign Y(k) before conversion input input limits saturated error successive times servoflag servo mode external status register move status register move flag saturation flag (1=pos,-1=neg) integrator
1997 Microchip Technology Inc. DS00532C-page
0000009D 000000A3 000000A7 000000AB 000000AF 000000B3 000000B7 000000B9 000000BB 000000BF 000000C1 000000C4
000000C5 000000C6 000000C8
000000CA
0000 0000 C021 0020 0020 C070
00095 00096 00097 00098 00099 00100 00101 00102 00103 00104 00105 00106 00107 00108 00109 00110 00111 00112 00113 00114 00115 00116 00117 00118 00119 00120 00121 00122 00123 00124 00125 00126 00127 00128 00129 00130 00131 00132 00133 00134 00135 00136 00137 00138 00139 00140 00141
C0,C01,C1,C11,C2,C21 DECVAL,DECSTAT,DECTMP,DECSIGN
difference equation coefficients decimal variables commanded commanded commanded commanded acceleration +-AL, velocity +-VL, position buffer velocity buffer
A,A1,A2,A3 V,V1,V2,V3 UPCOUNT,UPCOUNT1 DOWNCOUNT,DOWNCOUNT1 MOVDEL,MOVDEL1,MOVDEL2,MOVDEL3 PH2FLAT,PH2FLAT1 INDEXPOS,INDEXPOS1,INDEXPOS2 SHIFTNUM
running counter running down counter move discretization delta phase flat iteration counter position last index pulse shifts from middle
PICMASTER Debug/servo tuning Purposes Only CAPFLAG CAPCOUNT,CAPCOUNT1 CAPTMP,CAPTMP1 trace capture flag PICMASTER trace capture counter trace capture temporary storage
ZERO,ONE ENDC constants
GOTO GOTO
Startup 0x20 InterruptPoll
reset vector startup vector
interrupt vector
AN532
NAME: Startup DESCRIPTION: This routine called hardware reset when program wishes restore initial conditions. Initiali; zation run-time constants takes place here.
DS00532C-page 1997 Microchip Technology Inc.
AN532
0021 0021 8406
0022 0023 0024 0025
8404 8504 8604 8704
0026 B018 0027 4A01 0028 0028 2900 0029 1F01 002A C028 002B 15CB 002C B803 002D B03F 002E 770A 002F B07F 0030 720A 0031 730A 0032 B0C0 0033 700A 0034 710A 0035 B006 0036 760A 0037 B802 0038 B0FF 0039 740A 003A B00F
00142 00143 00144 00145 00146 00147 00148 00149 00150 00151 00152 00153 00154 00155 00156 00157 00158 00159 00160 00161 00162 00163 00164 00165 00166 00167 00168 00169 00170 00171 00172 00173 00174 00175 00176 00177 00178 00179 00180 00181 00182
RETURNS: restart safe initial state STACK UTILIZATION: none TIMING cycles): Startup AUTONO MOVLW MOVPF memloop CLRF INCFSZ GOTO INCF MOVLB MOVLW MOVFP MOVLW MOVFP MOVFP MOVLW MOVFP MOVFP MOVLW MOVFP MOVLB MOVLW MOVFP MOVLW INDF0, FSR0, memloop ONE, 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 organization timers BANK3 initialization _glintd disable interrupts auto increment decrement
_fs0 _fs1 _fs2 _fs3 0x18 WREG,FSR0 clear memory locations [18,FF]
duty cycle midpoint
BANK2 initialization
initialize timer1 period
1997 Microchip Technology Inc. DS00532C-page
003B 750A
003C B800 003D B080 003E 650A 003F B090 0040 730A 0041 B020 0042 750A 0043 B019 0044 770A 0045 B0F3 0046 710A 0047 B801 0048 B008 0049 4A27 004A B050 004B 4A29 004C B004 004D 4A2B 004E B001 004F 4A24 0050 B0F0 0051 4A21 0052 82C4 0053 5288 0054 0055 0056 0057 0058 0059 B080 4A8D B0FE 4A8E B040 4A8B
00183 00184 00185 00186 00187 00188 00189 00190 00191 00192 00193 00194 00195 00196 00197 00198 00199 00200 00201 00202 00203 00204 00205 00206 00207 00208 00209 00210 00211 00212 00213 00214 00215 00216 00217 00218 00219 00220 00221 00222 00223 00224 00225 00226 00227 00228 00229
MOVFP
WREG,PR2
initialize timer2 period
MOVLB MOVLW MOVFP MOVLW MOVFP MOVLW MOVFP MOVLW MOVFP MOVLW MOVFP MOVLB MOVLW MOVPF MOVLW MOVPF MOVLW MOVPF MOVLW MOVPF MOVLW MOVPF MOVPF MOVLW MOVPF MOVLW MOVPF MOVLW MOVPF
BANK0 T0STA_INIT WREG,RTCSTA RCSTA_INIT WREG,RCSTA TXSTA_INIT WREG,TXSTA SPBRG_INIT WREG,SPBRG DDRB_INIT WREG,DDRB BANK1 0x08 WREG,KP+B1 0x50 WREG,KV+B1 0x04 WREG,KI+B1 0x01 WREG,AL+B1 0xF0 WREG,VL+B1 SHIFTNUM,2 PW1DCH,YPWM+B1
BANK0 initialization
sets external input
receive status transmit status
baud rate
port whatever BANK1 initialization initialize some parameters proportional gain
derivative gain
integral gain
acceleration limit
velocity limit shift number
AN532
PWMAXL WREG,YPWMAX+B0 PWMAXH WREG,YPWMAX+B1 PWMINL WREG,YPWMIN+B0
initialize limits
DS00532C-page 1997 Microchip Technology Inc.
AN532
005A B001 005B 4A8C 005C 005D 005E 005F 2916 2907 8517 8307
0060 8C06 0061 B802 0062 0062 290B 0063 290C 0064 2912 0065 2913 0066 B0FF 0067 170A 0068 C067 0069 006A 006B 006C 006D 006E 6A0B 080C 0812 0813 330A C062
006F C086
0070 0070 4F3A 0071 4A3B 0072 B801 0073 E5EC
00230 00231 00232 00233 00234 00235 00236 00237 00238 00239 00240 00241 00242 00243 00244 00245 00246 00247 00248 00249 00250 00251 00252 00253 00254 00255 00256 00257 00258 00259 00260 00261 00262 00263 00264 00265 00266 00267 00268 00269 00270 00271 00272 00273 00274 00275 00276
MOVLW MOVPF CLRF CLRF MOVLB zeroctrs CLRF CLRF CLRF CLRF MOVLW DECFSZ GOTO MOVFP IORWF IORWF IORWF TSTFSZ GOTO GOTO
PWMINH WREG,YPWMIN+B1 PIR, INTSTA, _tm2ie _peie _glintd BANK2 clear flags, indiviual interrupts
enable interrupts
TMR0L, TMR0H, TMR3L, TMR3H, 0xFF WREG, delay TMR0L,WREG TMR0H,W TMR3L,W TMR3H,W WREG zeroctrs PollingLoop
clear counter
clear down counter
delay
motor still moving
NAME: InterruptPoll
InterruptPoll MOVPF MOVPF MOVLB CALL BSR,ISRBSR WREG,ISRWREG BANK1 doMPosMVel calculate measured position velocity save BSR,WREG
1997 Microchip Technology Inc. DS00532C-page
0074 E61D 0075 0076 0077 0078 0079 2298 B501 0499 9F0A E37E
007A 9E98 007B E44F 007C E291 007D 3395 007E E2D8 007F 33C5 0080 E742 0081 B801 0082 2916 0083 6F3A 0084 6A3B 0085 0005
0086 0086 0087 0088 0089 E08D E1AC 31CB C086
00277 00278 00279 00280 00281 00282 00283 00284 00285 00286 00287 00288 00289 00290 00291 00292 00293 00294 00295 00296 00297 00298 00299 00300 00301 00302 00303 00304 00305 00306 00307 00308 00309 00310 00311 00312 00313 00314 00315 00316 00317 00318 00319 00320 00321 00322 00323
CALL RLNCF ANDLW SUBWF BTFSC CALL BTFSC CALL CALL TSTFSZ CALL TSTFSZ CALL MOVLB CLRF MOVFP MOVFP RETFIE
doExtstat MOVSTAT,W 0x01 MOVFLAG,W WREG,MSB doPreMove MOVSTAT,BIT6 doMove doError SERVOFLAG doServo CAPFLAG doCaptureRegs BANK1 PIR, ISRBSR,BSR ISRWREG,WREG
evaluate external status MOVFLAG=0 MOVSTAT,BIT7=1 then premove. This only executed once beginning each move
motion continuing? move calculate position velocity error test servoflag, then servo servo
PIC-MASTER Trace Capture, demo purposes
clear interrupt request flags restore BSR,WREG
NAME: PollingLoop DESCRIPTION: actual polling loop called after board's initialization ENTRY CONDITIONS: System globals hardware initialized interrupt processes started.
AN532
PollingLoop CALL CALL CPFSEQ GOTO IdleFunction GetChk PollingLoop
GetChk, receive buffer full?
DS00532C-page 1997 Microchip Technology Inc.
AN532
008A E1A1 008B 4A3C 008C C08F
008D 008D 0004 008E 0002
008F 008F 0090 0091 0092 0093 0094 0094 0095 0096 B066 4A0D B007 4A0E AB3D A93D A23E A93F
0097 6A3D 0098 30CA 0099 C0A4 009A 313C 009B C094
00324 00325 00326 00327 00328 00329 00330 00331 00332 00333 00334 00335 00336 00337 00338 00339 00340 00341 00342 00343 00344 00345 00346 00347 00348 00349 00350 00351 00352 00353 00354 00355 00356 00357 00358 00359 00360 00361 00362 00363 00364 00365 00366 00367 00368 00369 00370
CALL MOVPF GOTO
GetChar WREG,CMDCHAR DoCommand
character CMDCHAR
NAME: IdleFunction DESCRIPTION: IdleFunction CLRWDT RETURN NAME: DoCommand DESCRIPTION: Search command table command execute DoCommand MOVLW MOVPF MOVLW MOVPF TABLRD tryNextCmd TABLRD TLRD TABLRD MOVFP CPFSLT GOTO CPFSEQ GOTO CMD_TABLE WREG,TBLPTRL HIGH CMD_TABLE WREG,TBLPTRH 1,1,CMDTEMP 0,1,CMDTEMP 1,CMDPTRH 0,1,CMDPTRL CMDTEMP,WREG ZERO noCommand CMDCHAR tryNextCmd error table read entry from table CMD_TABLE CMD_TABLE This routine will perform work while doing waits serial functions.
1997 Microchip Technology Inc. DS00532C-page
009C E1A4 009D 009E 009F 00A0 00A0 633E 623F 0000 E1A4
00A1 B00D 00A2 E1A4 00A3 C086 00A4 00A4 B03F 00A5 C0A0
00A6 00A6 B021 00A7 C0A0
00371 00372 00373 00374 00375 00376 00377 00378 00379 00380 00381 00382 00383 00384 00385 00386 00387 00388 00389 00390 00391 00392 00393 00394 00395 00396 00397 00398 00399 00400 00401 00402 00403 00404 00405 00406 00407 00408 00409 00410 00411 00412 00413 00414 00415 00416 00417
CALL MOVFP MOVFP cmdFinish CALL MOVLW CALL GOTO noCommand MOVLW GOTO
PutChar CMDPTRH,PCLATH CMDPTRL,PCL
echo command indirect jump command routine
PutChar PutChar PollingLoop
send response character from command routine followed
CMD_BAD cmdFinish
send error character
NAME: do_null DESCRIPTION: nothing command used determine chip working. Initiated carriage return. do_null MOVLW GOTO CMD_OK cmdFinish
NAME: do_move DESCRIPTION: ARGUMENTS: Commands axis move position velocity. Position data relative, encoder counts. Velocity data absolute, encoder counts/sample time multiplied 256. moves performed controller such that velocity acceleration limits into parameter memory will violated. move commands kept deep FIFO buffer. command buffer executed soon currently executed command complete.
AN532
[800000,7FFFFF]
DS00532C-page 1997 Microchip Technology Inc.
AN532
00A8
00A8 E217
00A9 9F98 00AA C0B4
00AB 00AC 00AD 00AE 00AF 00B0
6A31 4A5E 6A32 4A5F 6A33 4A60
00B1 8798 00B2 B021 00B3 C0A0 00B4 00B4 B03F 00B5 C0A0
00B6
00418 00419 00420 00421 00422 00423 00424 00425 00426 00427 00428 00429 00430 00431 00432 00433 00434 00435 00436 00437 00438 00439 00440 00441 00442 00443 00444 00445 00446 00447 00448 00449 00450 00451 00452 00453 00454 00455 00456
do_move CALL #else CALL #endif BTFSC GOTO MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF 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 buffer available GetVal DECIO GetDecVal
accept value into NMOVVAL byte into move b(B0) byte into move b(B1) byte into move b(B2)
buffer full flag
CMD_BAD cmdFinish
else, return error
NAME: do_mode DESCRIPTION: argument will cause subsequent move commands incremental position moves. argument will cause subsequent moves absolute velocity moves. ARGUMENTS: [P,V] do_mode
1997 Microchip Technology Inc. DS00532C-page
00B6 00B7 00B8 00B9 00BA 00BB 00BC 00BD 00BD 00BE 00BF 00C0 00C1 00C1 00C2 00C3 00C4 00C5 00C6 00C6 00C7 00C8 00C9 00CA 00CB 00CC 00CC 00CD 00CE 00CE 00CF
E08D E1AC 31CB C0B6 E1A1 4A50 2996 B050 3150 C0C1 C0CE B056 3150 C0C6 1596 C0CE B054 3150 C0CC 2B96 2995 C0CE B03F C0A0 6A50 E1A4
00D0 B021 00D1 C0A0
00457 00458 00459 00460 00461 00462 00463 00464 00465 00466 00467 00468 00469 00470 00471 00472 00473 00474 00475 00476 00477 00478 00479 00480 00481 00482 00483 00484 00485 00486 00487 00488 00489 00490 00491 00492 00493 00494 00495 00496 00497 00498 00499 00500 00501 00502 00503
CALL CALL CPFSEQ GOTO CALL MOVPF CLRF testP MOVLW CPFSEQ GOTO GOTO testV MOVLW CPFSEQ GOTO INCF GOTO testT MOVLW CPFSEQ GOTO SETF CLRF GOTO modeerror MOVLW GOTO modeok MOVFP CALL MOVLW GOTO
IdleFunction GetChk do_mode GetChar WREG,STRVALL MODETYPE, STRVALL testV modeok STRVALL testT MODETYPE, modeok STRVALL modeerror MODETYPE, SERVOFLAG, modeok CMD_BAD cmdFinish STRVALL,WREG PutChar CMD_OK cmdFinish
single character loop
MODETYPE=0 position moves position moves type
velocity moves type
MODETYPE=1 velocity moves
TORQUE Moves type
MODETYPE=-1 torque moves disable servo
mode error
echo type character
NAME: do_setparameter DESCRIPTION: Sets controller parameters value given. Parameter Range VL=velocity limit [0,7FFFFF] AL=acceleration limit [0,7FFFFF]
AN532
DS00532C-page 00D2 00D2 E253 00D3 B008 00D4 3031 00D5 C0F7 00D6 00D7 00D8 00D9 B089 4A0D B007 4A0E 00DA AB40 00DB 00DB A240 00DC A941 00DD A942 00DE B008 00DF 3040 00E0 C0F7 00E1 6A40 00E2 3131 00E3 C0DB 00E4 6A42 00E5 690A 1997 Microchip Technology Inc. 00E6 E217
AN532
00504 00505 00506 00507 00508 00509 00510 00511 00512 00513 00514 00515 00516 00517 00518 00519 00520 00521 00522 00523 00524 00525 00526 00527 00528 00529 00530 00531 00532 00533 00534 00535 00536 00537 00538 00539 00540 00541 00542 00543 00544 00545 00546 00547 00548 00549 00550
KP=proportional gain KP=velocity gain KP=integral gain IM=integrator mode FV=velocity FA=acceleration ARGUMENTS: [0,FF] [800000,7FFFFF] do_setparameter CALL MOVLW CPFSLT GOTO MOVLW MOVPF MOVLW MOVPF TABLRD setNextPar TLRD TABLRD TABLRD MOVLW CPFSLT GOTO MOVFP CPFSEQ GOTO MOVFP MOVFP CALL GetPar NUMPAR VALBUF+B0 Serror PAR_TABLE WREG,TBLPTRL HIGH PAR_TABLE WREG,TBLPTRH 1,1,PARTEMP
[8000,7FFF] [8000,7FFF] [8000,7FFF] [0,3] [8000,7FFF] Implemented [8000,7FFF] Implemented
parameter number check range [0,NUMPAR]
PAR_TABLE PAR_TABLE
1,PARTEMP 0,1,PARLEN 0,1,PARPTR NUMPAR PARTEMP Serror PARTEMP,WREG VALBUF+B0 setNextPar PARPTR,WREG WREG,FSR1 DECIO GetDecVal
read entry from table
error table
pointer parameter FSR1
value VALBUF
1997 Microchip Technology Inc. DS00532C-page
00E7 B031 00E8 610A
00E9 00EA 00EB 00EC 00ED 00ED 00EE 00EF 00F0
8404 8D04 8604 8F04
6800 0741 3341 C0ED
00F1 00F2 00F3 00F4
8404 8504 8604 8704
00F5 B021 00F6 C0A0 00F7 00F7 B03F 00F8 C0A0
00551 00552 00553 00554 00555 00556 00557 00558 00559 00560 00561 00562 00563 00564 00565 00566 00567 00568 00569 00570 00571 00572 00573 00574 00575 00576 00577 00578 00579 00580 00581 00582 00583 00584 00585
#else CALL #endif MOVLW VALBUF MOVFP WREG,FSR0 AUTOINC setGetMore MOVFP DECF TSTFSZ GOTO AUTONO _fs0 _fs1 _fs2 _fs3 _fs0 _fs1 _fs2 _fs3 pointer VALBUF FSR0 autoincrement GetVal
INDF0,INDF1 PARLEN, PARLEN setGetMore
move value parameter
autoincrement
MOVLW GOTO Serror MOVLW GOTO
CMD_OK cmdFinish
CMD_BAD cmdFinish
AN532
NAME: do_readparameter DESCRIPTION: Returns present value parameter. ARGUMENTS: [0,FF]
DS00532C-page 1997 Microchip Technology Inc.
AN532
00F9 00F9 E253 00FA B008 00FB 3031 00FC C121 00FD 00FE 00FF 0100 B089 4A0D B007 4A0E
0101 AB40 0102 0102 A240 0103 A941 0104 A942 0105 B008 0106 3040 0107 C121 0108 6A40 0109 3131 010A C102 010B 6A42 010C 690A 010D B031 010E 610A
010F 0110 0111 0112
8404 8D04 8604 8F04
0113 2931 0114 2932 0115 2933
00586 RETURNS: present value requested parameter returned. 00587 00588 do_readparameter 00589 00590 CALL GetPar parameter number 00591 00592 MOVLW NUMPAR check range [0,NUMPAR] 00593 CPFSLT VALBUF+B0 00594 GOTO Rerror 00595 00596 MOVLW PAR_TABLE PAR_TABLE 00597 MOVPF WREG,TBLPTRL 00598 MOVLW HIGH PAR_TABLE PAR_TABLE 00599 MOVPF WREG,TBLPTRH 00600 00601 TABLRD 1,1,PARTEMP 00602 00603 readNextPar 00604 TLRD 1,PARTEMP read entry from table 00605 TABLRD 0,1,PARLEN 00606 TABLRD 0,1,PARPTR 00607 00608 MOVLW NUMPAR error table 00609 CPFSLT PARTEMP 00610 GOTO Rerror 00611 00612 MOVFP PARTEMP,WREG 00613 CPFSEQ VALBUF+B0 00614 GOTO readNextPar 00615 00616 MOVFP PARPTR,WREG pointer parameter FSR1 00617 MOVFP WREG,FSR1 00618 00619 MOVLW VALBUF pointer VALBUF FSR1 00620 MOVFP WREG,FSR0 00621 AUTOINC autoincrement _fs0 _fs1 _fs2 _fs3 00622 00623 CLR24 VALBUF clear VALBUF CLRF VALBUF+B0, CLRF VALBUF+B1, CLRF VALBUF+B2,
1997 Microchip Technology Inc. DS00532C-page
0116 0116 0117 0118 0119
6008 0741 3341 C116
011A 011B 011C 011D
8404 8504 8604 8704
011E E260
011F B021 0120 C0A0 0121 0121 B03F 0122 C0A0
00624 00625 00626 00627 00628 00629 00630 00631 00632 00633 00634 00635 00636 00637 00638 00639 00640 00641 00642 00643 00644 00645 00646 00647 00648 00649 00650 00651 00652 00653 00654 00655 00656 00657 00658 00659 00660 00661 00662 00663
readGetMore MOVFP DECF TSTFSZ GOTO AUTONO
INDF1,INDF0 PARLEN, PARLEN readGetMore
read parameter into VALBUF
autoincrement _fs0 _fs1 _fs2 _fs3
CALL #else CALL #endif MOVLW GOTO Rerror MOVLW GOTO
DECIO PutDecVal
send parameter value
PutVal
CMD_OK cmdFinish
CMD_BAD cmdFinish
NAME: do_shutter DESCRIPTION: Returns time sample time counts [0,FFFF]) since start present move captures commanded measured values position velocity time command. ARGUMENTS: RETURNS: time since start present move returned.
AN532
DS00532C-page 1997 Microchip Technology Inc.
AN532
0123 0123 8406
0124 0125 0126 0127 0128 0129
6A58 4A43 6A59 4A44 6A5A 4A45
012A 012B 012C 012D 012E 012F
6A5B 4A46 6A5C 4A47 6A5D 4A48
0130 0131 0132 0133 0134 0135
6A75 4A49 6A76 4A4A 6A77 4A4B
0136 0137 0138 0139 013A 013B
6A78 4A4C 6A79 4A4D 6A7A 4A4E
013C 2933
013D 6A6A
00664 00665 00666 do_shutter 00667 00668 00669 00670 MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF 00671 MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF 00672 MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF 00673 MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF 00674 00675 CLRF 00676 MOV16 MOVFP
_glintd POSITION,CPOSITION POSITION+B0,WREG WREG,CPOSITION+B0 POSITION+B1,WREG WREG,CPOSITION+B1 POSITION+B2,WREG WREG,CPOSITION+B2 VELOCITY,CVELOCITY VELOCITY+B0,WREG WREG,CVELOCITY+B0 VELOCITY+B1,WREG WREG,CVELOCITY+B1 VELOCITY+B2,WREG WREG,CVELOCITY+B2 MPOSITION,CMPOSITION MPOSITION+B0,WREG WREG,CMPOSITION+B0 MPOSITION+B1,WREG WREG,CMPOSITION+B1 MPOSITION+B2,WREG WREG,CMPOSITION+B2 MVELOCITY,CMVELOCITY MVELOCITY+B0,WREG WREG,CMVELOCITY+B0 MVELOCITY+B1,WREG WREG,CMVELOCITY+B1 MVELOCITY+B2,WREG WREG,CMVELOCITY+B2
disable interrupts capture commanded position byte into move b(B0) byte into move b(B1) byte into move b(B2)
capture commanded velocity byte into move b(B0) byte into move b(B1) byte into move b(B2)
capture measured position byte into move b(B0) byte into move b(B1) byte into move b(B2)
capture measured velocity byte into move b(B0) byte into move b(B1) byte into move b(B2)
VALBUF+B2, MOVTIME,VALBUF MOVTIME+B0,WREG
capture move time, move VALBUF byte into
1997 Microchip Technology Inc. DS00532C-page
013E 4A31 013F 6A6B 0140 4A32
0141 8C06
0142 E260
0143 B021 0144 C0A0
0145
0145 0146 0147 0148 0149 014A
6A43 4A31 6A44 4A32 6A45 4A33
00677 00678 00679 00680 00681 00682 00683 00684 00685 00686 00687 00688 00689 00690 00691 00692 00693 00694 00695 00696 00697 00698 00699 00700 00701 00702 00703 00704 00705 00706 00707 00708 00709 00710 00711
MOVPF MOVFP MOVPF
WREG,VALBUF+B0 MOVTIME+B1,WREG WREG,VALBUF+B1
move b(B0) byte into move b(B1)
CALL #else CALL #endif MOVLW GOTO
_glintd DECIO PutDecVal
enable interrupts
PutVal
CMD_OK cmdFinish
NAME: do_readcomposition DESCRIPTION: Returns commanded position count which captured during last shutter command. ARGUMENTS: RETURNS: last captured position count returned. [800000,7FFFFF] do_readcomposition MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CPOSITION,VALBUF CPOSITION+B0,WREG WREG,VALBUF+B0 CPOSITION+B1,WREG WREG,VALBUF+B1 CPOSITION+B2,WREG WREG,VALBUF+B2 move CPOSITION VALBUF byte into move b(B0) byte into move b(B1) byte into move b(B2)
AN532
DECIO
DS00532C-page 1997 Microchip Technology Inc.
AN532
014B E260
014C B021 014D C0A0
014E
014E 014F 0150 0151 0152 0153
6A46 4A31 6A47 4A32 6A48 4A33
0154 E260
00712 00713 00714 00715 00716 00717 00718 00719 00720 00721 00722 00723 00724 00725 00726 00727 00728 00729 00730 00731 00732 00733 00734 00735 00736 00737 00738 00739 00740 00741 00742 00743 00744 00745 00746 00747 00748 00749 00750
CALL #else CALL #endif MOVLW GOTO
PutDecVal
PutVal
CMD_OK cmdFinish
NAME: do_readcomvelocity DESCRIPTION: Returns commanded velocity multiplied which captured during last shutter command. ARGUMENTS: RETURNS: last captured commanded velocity times returned. [800000,7FFFFF] do_readcomvelocity MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CVELOCITY,VALBUF CVELOCITY+B0,WREG WREG,VALBUF+B0 CVELOCITY+B1,WREG WREG,VALBUF+B1 CVELOCITY+B2,WREG WREG,VALBUF+B2 move commanded velocity VALBUF byte into move b(B0) byte into move b(B1) byte into move b(B2)
CALL #else CALL #endif
DECIO PutDecVal
PutVal
1997 Microchip Technology Inc. DS00532C-page
0155 B021 0156 C0A0
0157
0157 0158 0159 015A 015B 015C
6A49 4A31 6A4A 4A32 6A4B 4A33
015D E260
015E B021 015F C0A0
00751 00752 00753 00754 00755 00756 00757 00758 00759 00760 00761 00762 00763 00764 00765 00766 00767 00768 00769 00770 00771 00772 00773 00774 00775 00776 00777 00778 00779 00780 00781 00782 00783 00784 00785 00786 00787 00788 00789
MOVLW GOTO
CMD_OK cmdFinish
NAME: do_readactposition DESCRIPTION: Returns measured position count which captured during last shutter command. ARGUMENTS: RETURNS: last captured measured position count returned. [800000,7FFFFF] do_readactposition MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CMPOSITION,VALBUF CMPOSITION+B0,WREG WREG,VALBUF+B0 CMPOSITION+B1,WREG WREG,VALBUF+B1 CMPOSITION+B2,WREG WREG,VALBUF+B2 move measured position VALBUF byte into move b(B0) byte into move b(B1) byte into move b(B2)
CALL #else CALL #endif MOVLW GOTO
DECIO PutDecVal
PutVal
CMD_OK cmdFinish
AN532
NAME: do_readactvelocity
DS00532C-page 0160 0160 0161 0162 0163 0164 0165 6A4C 4A31 6A4D 4A32 6A4E 4A33 0166 E260 0167 B021 0168 C0A0 1997 Microchip Technology Inc.
AN532
00790 00791 00792 00793 00794 00795 00796 00797 00798 00799 00800 00801 00802 00803 00804 00805 00806 00807 00808 00809 00810 00811 00812 00813 00814 00815 00816 00817 00818 00819 00820 00821 00822 00823 00824 00825 00826 00827 00828
DESCRIPTION: ARGUMENTS: RETURNS:
Returns measured velocity multiplied which captured during last shutter command. last captured measured velocity times returned. [800000,7FFFFF]
do_readactvelocity MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CMVELOCITY,VALBUF CMVELOCITY+B0,WREG WREG,VALBUF+B0 CMVELOCITY+B1,WREG WREG,VALBUF+B1 CMVELOCITY+B2,WREG WREG,VALBUF+B2 move measured velocity VALBUF byte into move b(B0) byte into move b(B1) byte into move b(B2)
CALL #else CALL #endif MOVLW GOTO
DECIO PutDecVal
PutVal
CMD_OK cmdFinish
NAME: do_externalstatus DESCRIPTION: Returns digit number which defines state bits external status register. Issuing this command will clear bits external status register unless event which still true. ARGUMENTS:
1997 Microchip Technology Inc. DS00532C-page
0169 0169 016A 016B 016C 016D 8406 6A97 2997 8C06 E1B3
016E B021 016F C0A0
0170 0170 6A98 0171 E1B3 0172 B021 0173 C0A0
00829 00830 00831 00832 00833 00834 00835 00836 00837 00838 00839 00840 00841 00842 00843 00844 00845 00846 00847 00848 00849 00850 00851 00852 00853 00854 00855 00856 00857 00858 00859 00860 00861 00862 00863 00864 00865 00866 00867 00868 00869 00870 00871 00872 00873 00874 00875
RETURNS:
external status register returned.
do_externalstatus MOVFP CLRF CALL MOVLW GOTO _glintd EXTSTAT,WREG EXTSTAT, _glintd PutHex CMD_OK cmdFinish
NAME: do_movestatus DESCRIPTION: Returns digit number which defines state bits move status register. Issuing this command will clear bits move status register unless event which still true. ARGUMENTS: RETURNS: move status register returned. do_movestatus MOVFP CALL MOVLW GOTO MOVSTAT,WREG PutHex CMD_OK cmdFinish
NAME: do_readindposition DESCRIPTION: Returns last index position captured position counts. ARGUMENTS: RETURNS: last captured index position returned.
AN532
DS00532C-page 1997 Microchip Technology Inc.
AN532
0174
0174 0175 0176 0177 0178 0179
6AC1 4A31 6AC2 4A32 6AC3 4A33
017A E260
017B B021 017C C0A0
017D
017D E217
00876 00877 00878 00879 00880 00881 00882 00883 00884 00885 00886 00887 00888 00889 00890 00891 00892 00893 00894 00895 00896 00897 00898 00899 00900 00901 00902 00903 00904 00905 00906 00907 00908 00909 00910 00911 00912 00913 00914
do_readindposition MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF INDEXPOS,VALBUF INDEXPOS+B0,WREG WREG,VALBUF+B0 INDEXPOS+B1,WREG WREG,VALBUF+B1 INDEXPOS+B2,WREG WREG,VALBUF+B2 move measured velocity VALBUF byte into move b(B0) byte into move b(B1) byte into move b(B2)
CALL #else CALL #endif MOVLW GOTO
DECIO PutDecVal
PutVal
CMD_OK cmdFinish
NAME: do_setposition DESCRIPTION: Sets measured commanded position value given. This command should sent unless move FIFO buffer empty. ARGUMENTS: [800000,7FFFFF] do_setposition CALL #else CALL GetVal DECIO GetDecVal
1997 Microchip Technology Inc. DS00532C-page
017E 017F 0180 0181 0182 0183
6A31 4A58 6A32 4A59 6A33 4A5A
0184 0185 0186 0187 0188 0189
6A31 4A75 6A32 4A76 6A33 4A77
018A 018B 018C 018D
2983 2984 2985 2986
018E B021 018F C0A0
0190 0190 B021 0191 E1A4 0192 C021
00915 00916 00917 00918 00919 00920 00921 00922 00923 00924 00925 00926 00927 00928 00929 00930 00931 00932 00933 00934 00935 00936 00937 00938 00939 00940
#endif MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF VALBUF,POSITION VALBUF+B0,WREG WREG,POSITION+B0 VALBUF+B1,WREG WREG,POSITION+B1 VALBUF+B2,WREG WREG,POSITION+B2 VALBUF,MPOSITION VALBUF+B0,WREG WREG,MPOSITION+B0 VALBUF+B1,WREG WREG,MPOSITION+B1 VALBUF+B2,WREG WREG,MPOSITION+B2 byte into move b(B0) byte into move b(B1) byte into move b(B2) byte into move b(B0) byte into move b(B1) byte into move b(B2)
CLR32 CLRF CLRF CLRF CLRF
Y+B0, Y+B1, Y+B2, Y+B3,
MOVLW GOTO
CMD_OK cmdFinish
NAME: do_reset DESCRIPTION: Performs software reset. ARGUMENTS: do_reset MOVLW CALL GOTO CMD_OK PutChar Startup
AN532
DS00532C-page 1997 Microchip Technology Inc.
AN532
0193 0193 2995 0194 B021 0195 C0A0
0196
0196 E217
0197 0198 0199 019A
6A31 4AC6 6A32 4AC7
019B 019C 019D 019E
6A31 4AC8 6A32 4AC9
00941 00942 00943 00944 00945 00946 00947 00948 00949 00950 00951 00952 00953 00954 00955 00956 00957 00958 00959 00960 00961 00962 00963 00964 00965 00966 00967 00968 00969 00970 00971 00972 00973 00974 00975
NAME: do_stop DESCRIPTION: Stops servo clearing SERVOFLAG. do_stop CLRF MOVLW GOTO SERVOFLAG, CMD_OK cmdFinish
NAME: do_capture do_capture CALL #else CALL #endif MOV16 MOVFP MOVPF MOVFP MOVPF MOV16 MOVFP MOVPF MOVFP MOVPF VALBUF,CAPCOUNT VALBUF+B0,WREG WREG,CAPCOUNT+B0 VALBUF+B1,WREG WREG,CAPCOUNT+B1 VALBUF,CAPTMP VALBUF+B0,WREG WREG,CAPTMP+B0 VALBUF+B1,WREG WREG,CAPTMP+B1 byte into move b(B0) byte into move b(B1) byte into move b(B0) byte into move b(B1) GetVal DECIO GetDecVal
1997 Microchip Technology Inc. DS00532C-page
019F B021 01A0 C0A0
01A1 01A1 B800 01A2 540A 01A3 0002
01A4 01A4 B801 01A5 01A5 9116 01A6 C1A5 01A7 B800 01A8 01A8 9115 01A9 C1A8 01AA 4A16 01AB 0002
00976 00977 00978 00979 00980 00981 00982 00983 00984 00985 00986 00987 00988 00989 00990 00991 00992 00993 00994 00995 00996 00997 00998 00999 01000 01001 01002 01003 01004 01005 01006 01007 01008 01009 01010 01011 01012 01013 01014 01015 01016 01017 01018 01019 01020 01021 01022
MOVLW GOTO
CMD_OK cmdFinish
NAME: GetChar DESCRIPTION: character from receive buffer. GetChar MOVLB MOVPF RETURN NAME: PutChar DESCRIPTION: send character serial port ARGUMENTS: WREG contains byte transmitted PutChar MOVLB bufwait BTFSS GOTO MOVLB shfwait BTFSS GOTO MOVPF RETURN _trmt shfwait WREG,TXREG transmit shift register empty? _tbmt bufwait BANK0 bank0 BANK1 bank1 transmit buffer empty? BANK0 RCREG,WREG bank0 receive character
send character
AN532
NAME: GetChk DESCRIPTION: Check character receive buffer.
DS00532C-page 1997 Microchip Technology Inc.
AN532
01AC 01AC 01AD 01AE 01AF
B801 560A B501 0002
01B0 01B0 B130 01B1 E1A4 01B2 0002
01B3 01B3 01B4 01B5 01B6 4A51 1D0A B50F 4A52
01023 01024 01025 01026 01027 01028 01029 01030 01031 01032 01033 01034 01035 01036 01037 01038 01039 01040 01041 01042 01043 01044 01045 01046 01047 01048 01049 01050 01051 01052 01053 01054 01055 01056 01057 01058 01059 01060 01061 01062 01063 01064 01065 01066 01067 01068 01069
GetChk MOVLB MOVPF ANDLW RETURN BANK1 PIR,WREG CHARREADY bank1 return status WREG
NAME: PutDec DESCRIPTION: Converts value [0,F] WREG ASCII equivalent. upper nibble WREG assumed zero. ENTRY CONDITIONS: WREG value converted sent ASCII decimal PutDec ADDLW CALL RETURN #endif NAME: PutHex DESCRIPTION: Convert WREG value ASCII hexadecimal. output format digits with parts upper case leading zeros. result sent serial port with PutChar. ENTRY CONDITIONS: WREG value converted sent ASCII PutHex MOVPF SWAPF ANDLW MOVPF WREG,HEXVAL WREG, 0x0F WREG,HEXTMP 0x30 PutChar convert ASCII DECIO
1997 Microchip Technology Inc. DS00532C-page
01B7 01B8 01B9 01BA 01BB 01BC 01BD 01BE 01BE 01BF 01C0 01C0 01C1 01C2 01C3 01C4 01C5 01C6 01C7 01C8 01C9 01CA 01CB 01CB 01CC 01CD 01CD
2D0A B109 970A C1BE B037 0E52 C1C0 B030 0E52 E1A4 6A51 B50F 4A52 2D0A B109 970A C1CB B037 0E52 C1CD B030 0E52 E1A4
01CE 0002
01CF 01CF AB4F 01D0 01D0 A24F 01D1 A950 01D2 6A4F 01D3 31CA
01070 01071 01072 01073 01074 01075 01076 01077 01078 01079 01080 01081 01082 01083 01084 01085 01086 01087 01088 01089 01090 01091 01092 01093 01094 01095 01096 01097 01098 01099 01100 01101 01102 01103 01104 01105 01106 01107 01108 01109 01110 01111 01112 01113 01114 01115 01116
NEGW ADDLW BTFSS GOTO MOVLW ADDWF GOTO puth20 MOVLW ADDWF puth25 CALL MOVFP ANDLW MOVPF NEGW ADDLW BTFSS GOTO MOVLW ADDWF GOTO putl20 MOVLW ADDWF putl25 CALL RETURN
WREG, 0x09 WREG,MSB puth20 `A'-0x0A HEXTMP,W puth25 HEXTMP,W PutChar HEXVAL,WREG 0x0F WREG,HEXTMP WREG, 0x09 WREG,MSB putl20 `A'-0x0A HEXTMP,W putl25 HEXTMP,W PutChar
NAME: PutStr DESCRIPTION: Sends character string serial port. PutStr
AN532
TABLRD GetNextPair TLRD TABLRD MOVFP CPFSEQ
1,1,STRVALH 1,STRVALH 0,1,STRVALL STRVALH,WREG ZERO
DS00532C-page 1997 Microchip Technology Inc.
AN532
01D4 C1D6 01D5 0002 01D6 01D6 E1A4 01D7 01D8 01D9 01DA 01DB 01DB 6A50 31CA C1DB 0002 E1A4
01DC C1D0
01DD 01DD 01DD 01DE 01DF 01E0 01E1 01E2 01E3 01E4 01E5 01E6 01E7 01E8 01E9 01EA 01EA 01EB 01EC 01ED
E08D E1AC 31CB C1DD 2953 E1A1 4A51 E1A4 B00D 0451 330A C1EA C1F4
6A51 B239 970A C1F0
01117 01118 01119 01120 01121 01122 01123 01124 01125 01126 01127 01128 01129 01130 01131 01132 01133 01134 01135 01136 01137 01138 01139 01140 01141 01142 01143 01144 01145 01146 01147 01148 01149 01150 01151 01152 01153 01154 01155 01156 01157 01158 01159 01160 01161 01162 01163
GOTO RETURN putH CALL MOVFP CPFSEQ GOTO RETURN putL CALL GOTO
putH
PutChar STRVALL,WREG ZERO putL
PutChar GetNextPair
NAME: GetHex DESCRIPTION: Receive ASCII character from serial port convert numerical value. RETURNS: numerical value HEXVAL GetHex getnxt CALL CALL CPFSEQ GOTO CLRF CALL MOVPF CALL MOVLW SUBWF TSTFSZ GOTO GOTO gth10 MOVFP SUBLW BTFSS GOTO HEXVAL,WREG WREG,MSB gth20 IdleFunction GetChk getnxt HEXSTAT, GetChar WREG,HEXVAL PutChar HEXVAL,W WREG gth10 gthCR
1997 Microchip Technology Inc. DS00532C-page
01EE B009 01EF 0F51 01F0 01F0 01F1 01F2 01F3
B00F 0B51 2953 0002
01F4 01F4 B001 01F5 4A53 01F6 0002
01F7 01F7 01F7 01F8 01F9 01FA
E08D E1AC 31CB C1F7
01FB E1A1 01FC 4AA3 01FD E1A4 01FE 01FF 0200 0201 0202 0203 0204 B00D 04A3 30CA C20E B02D 04A3 30CA
01164 01165 01166 01167 01168 01169 01170 01171 01172 01173 01174 01175 01176 01177 01178 01179 01180 01181 01182 01183 01184 01185 01186 01187 01188 01189 01190 01191 01192 01193 01194 01195 01196 01197 01198 01199 01200 01201 01202 01203 01204 01205 01206 01207 01208 01209 01210
MOVLW ADDWF gth20 MOVLW ANDWF CLRF RETURN gthCR MOVLW MOVPF RETURN
0x09 HEXVAL,
0x0F HEXVAL, HEXSTAT,
0x01 WREG,HEXSTAT
NAME: GetDec DESCRIPTION: Receive ASCII decimal character from serial port convert numerical value. ARGUMENTS: numerical value returned DECVAL GetDec getdecnxt CALL CALL CPFSEQ GOTO CALL MOVPF CALL MOVLW SUBWF CPFSLT GOTO MOVLW SUBWF CPFSLT DECIO
IdleFunction GetChk getdecnxt GetChar WREG,DECVAL PutChar
AN532
DECVAL,W ZERO gtdCR DECVAL,W ZERO
DS00532C-page 1997 Microchip Technology Inc.
AN532
0205 0206 0207 0208 0209 020A 020A 020B 020C 020D 020E 020E 020F 0210 0211 0211 0212 0213 0214 0214 0215 0216
C211 B020 04A3 30CA C214 B00F 0BA3 29A4 0002 B002 4AA4 0002 B001 4AA4 0002 B000 4AA4 0002
01211 01212 01213 01214 01215 01216 01217 01218 01219 01220 01221 01222 01223 01224 01225 01226 01227 01228 01229 01230 01231 01232 01233 01234 01235 01236 01237 01238 01239 01240 01241 01242 01243 01244 01245 01246 01247 01248 01249 01250 01251 01252 01253 01254 01255 01256 01257
GOTO MOVLW SUBWF CPFSLT GOTO gtd09 MOVLW ANDWF CLRF RETURN gtdCR MOVLW MOVPF RETURN gtdMN MOVLW MOVPF RETURN gtdSP MOVLW MOVPF RETURN #endif
gtdMN DECVAL,W ZERO gtdSP 0x0F DECVAL, DECSTAT,
DEC_CR WREG,DECSTAT
DEC_MN WREG,DECSTAT
DEC_SP WREG,DECSTAT
NAME: getval DESCRIPTION: value [800000,7FFFFF] from serial port place VALBUF. DECIO #else GetVal CLR24 getnext CALL MOVLW CPFSEQ GOTO RETURN shift SWAPF VALBUF+B2 GetHex 0x01 HEXSTAT shift VALBUF
1997 Microchip Technology Inc. 0217 0217 2931 0218 2932 0219 2933 DS00532C-page 021A 021B 021C 021D 021E E1F7 2BA6 B001 31A4 29A6
01258 01259 01260 01261 01262 01263 01264 01265 01266 01267 01268 01269 01270 01271 01272 01273 01274 01275 01276 01277 01278 01279 01280 01281 01282 01283 01284 01285 01286 01287 01288 01289 01290 01291 01292 01293 01294 01295 01296 01297 01298 01299 01300
MOVFP ANDLW MOVPF SWAPF MOVFP ANDLW ADDWF MOVFP ANDLW MOVPF SWAPF MOVFP ANDLW ADDWF MOVFP ANDLW ADDWF MOVPF GOTO #endif
VALBUF+B2,WREG 0xF0 WREG,VALBUF+B2 VALBUF+B1 VALBUF+B1,WREG 0x0F VALBUF+B2, VALBUF+B1,WREG 0xF0 WREG,VALBUF+B1 VALBUF+B0 VALBUF+B0,WREG 0x0F VALBUF+B1 VALBUF+B0,WREG 0xF0 HEXVAL,W WREG,VALBUF+B0 getnext
NAME: GetDecVal DESCRIPTION: value [-8388608,8388607] from serial port place VALBUF RETURNS: numerical value returned VALBUF GetDecVal CLR24 CLRF CLRF CLRF CALL SETF MOVLW CPFSEQ CLRF DECIO
VALBUF VALBUF+B0, VALBUF+B1, VALBUF+B2,
AN532
GetDec DECSIGN, DEC_MN DECSTAT DECSIGN,
DS00532C-page 1997 Microchip Technology Inc.
AN532
021F 021F E1F7 0220 0221 0222 0223 0224 B002 31A4 C224 C248
0224 0225 0226 0227
8804 1B31 1B32 1B33
0228 0229 022A 022B 022C 022D
6A31 4A37 6A32 4A38 6A33 4A39
022E 022F 0230 0231
8804 1B31 1B32 1B33
0232 0233 0234 0235
8804 1B31 1B32 1B33
0236 0237 0238 0239 023A 023B
6A37 0F31 6A38 1132 6A39 1133
01301 getdecnext 01302 CALL 01303 01304 MOVLW 01305 CPFSEQ 01306 GOTO 01307 GOTO 01308 mul10 01309 01310 RLC24 RLCF RLCF RLCF 01311 MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF 01312 RLC24 RLCF RLCF RLCF 01313 RLC24 RLCF RLCF RLCF 01314 ADD24 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC
GetDec DEC_CR DECSTAT mul10 fixsign
VALBUF _carry VALBUF+B0, VALBUF+B1, VALBUF+B2, VALBUF,DVALBUF VALBUF+B0,WREG WREG,DVALBUF+B0 VALBUF+B1,WREG WREG,DVALBUF+B1 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,
multiply VALBUF
save DVALBUF byte into move b(B0) byte into move b(B1) byte into move b(B2)
VALBUF multiplied eight
VALBUF multiplied lowest byte lowest byte byte byte byte byte into save b(B0) into save b(B1) into save b(B2)
1997 Microchip Technology Inc. DS00532C-page
023C 2937 023D 2938 023E 2939 023F 6AA3 0240 4A37
0241 0242 0243 0244 0245 0246 0247 0248 0248 0249 024A
6A37 0F31 6A38 1132 6A39 1133 C21F 290A 32A6 0002
024B 024C 024D 024E 024F 0250 0251
1331 1332 1333 290A 1531 1132 1133
0252 0002
0253
01315 01316 01317 01318 01319 01320 01321 01322 01323 01324 01325 01326 01327 01328 01329 01330 01331 01332 01333 01334 01335 01336 01337 01338 01339
CLR24 CLRF CLRF CLRF MOVFP MOVPF ADD24 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC GOTO fixsign CLRF CPFSGT RETURN NEG24 COMF COMF COMF CLRF INCF ADDWFC ADDWFC RETURN #endif
DVALBUF DVALBUF+B0, 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 WREG, DECSIGN VALBUF VALBUF+B0, VALBUF+B1, VALBUF+B2, WREG, VALBUF+B0, VALBUF+B1, VALBUF+B2, lowest byte lowest byte byte byte byte byte into save b(B0) into save b(B1) into save b(B2)
NAME: GetPar DESCRIPTION: parameter number [0,FF] from serial port place VALBUF+B0. GetPar
AN532
DS00532C-page 1997 Microchip Technology Inc.
AN532
0253 2931 0254 2932 0255 2933
0256 0257 0258 0259 025A
E1DD 6A51 B50F 4A31 1D31
025B E1DD 025C 6A31 025D 0E51 025E 4A31 025F 0002
01340 01341 01342 01343 01344 01345 01346 01347 01348 01349 01350 01351 01352 01353 01354 01355 01356 01357 01358 01359 01360 01361 01362 01363 01364 01365 01366 01367 01368 01369 01370 01371 01372 01373 01374 01375 01376 01377 01378 01379 01380 01381 01382
CLR24 CLRF CLRF CLRF
VALBUF VALBUF+B0, VALBUF+B1, VALBUF+B2,
CALL MOVFP ANDLW MOVPF SWAPF CALL MOVFP ADDWF MOVPF RETURN
GetHex HEXVAL,WREG 0x0F WREG,VALBUF+B0 VALBUF+B0, GetHex VALBUF+B0,WREG HEXVAL,W WREG,VALBUF+B0
NAME: PutVal DESCRIPTION: Sends value VALBUF [800000,7FFFFF] serial port. #else PutVal MOVFP CALL MOVFP CALL MOVFP CALL RETURN #endif VALBUF+B2,WREG PutHex VALBUF+B1,WREG PutHex VALBUF+B0,WREG PutHex DECIO
1997 Microchip Technology Inc. DS00532C-page
0260 0260 9733 0261 C26C
0262 0263 0264 0265 0266 0267 0268 0269 026A 026B 026C 026C 026D 026E 026E 026F 0270 0271 0272 0273 0273 0274 0275 0276 0277 0278 0279 027A 027A 027B
1331 1332 1333 290A 1531 1132 1133 B02D E1A4 C26E B020 E1A4
B09A 4A0D B007 4A0E A937 A037 AB38 A939 2B0A 3137 C27A C28E 1537 2BA3
01383 01384 01385 01386 01387 01388 01389 01390 01391 01392 01393 01394 01395 01396 01397 01398 01399 01400 01401 01402 01403 01404 01405 01406 01407 01408 01409 01410 01411 01412 01413 01414 01415 01416 01417 01418 01419 01420
NAME: DESCRIPTION: PutDecVal BTFSS GOTO NEG24 COMF COMF COMF CLRF INCF ADDWFC ADDWFC MOVLW CALL GOTO pdpos MOVLW CALL pddigits MOVLW MOVPF MOVLW MOVPF TABLRD readNextDec TLRD TABLRD TABLRD SETF CPFSEQ GOTO GOTO getdigit INCF SETF
PutDecVal Send value VALBUF [-8388608,8388607] serial port.
DECIO
VALBUF+B2,MSB pdpos VALBUF VALBUF+B0, VALBUF+B1, VALBUF+B2, WREG, VALBUF+B0, VALBUF+B1, VALBUF+B2, PutChar pddigits PutChar
DEC_TABLE WREG,TBLPTRL HIGH DEC_TABLE WREG,TBLPTRH 0,1,DVALBUF+B0 0,DVALBUF+B0 1,1,DVALBUF+B1 0,1,DVALBUF+B2 WREG, DVALBUF+B0 getdigit unitsposition DVALBUF+B0, DECVAL,
DEC_TABLE DEC_TABLE
read entry from table
AN532
unitsposition table
restore power DECVAL
DS00532C-page 1997 Microchip Technology Inc.
AN532
027C 027C 15A3
027D 027E 027F 0280 0281 0282
6A37 0531 6A38 0332 6A39 0333
0283 9733 0284 C27C
0285 0286 0287 0288 0289 028A
6A37 0F31 6A38 1132 6A39 1133
028B 6AA3 028C E1B0 028D C273 028E 028E 6A31 028F E1B0 0290 0002
0291
01421 01422 01423 01424 01425 01426 01427 01428 01429 01430 01431 01432 01433 01434 01435 01436 01437 01438 01439 01440 01441 01442 01443 01444 01445 01446 01447 01448 01449 01450 01451
INCF SUB24 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB BTFSS GOTO ADD24 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP CALL GOTO unitsposition MOVFP CALL RETURN DECVAL, DVALBUF,VALBUF DVALBUF+B0,WREG VALBUF+B0, DVALBUF+B1,WREG VALBUF+B1, DVALBUF+B2,WREG VALBUF+B2, VALBUF+B2,MSB DVALBUF,VALBUF DVALBUF+B0,WREG VALBUF+B0, DVALBUF+B1,WREG VALBUF+B1, DVALBUF+B2,WREG VALBUF+B2, DECVAL,WREG PutDec readNextDec correct VALBUF next digit lowest byte lowest byte byte byte byte byte into save b(B0) into save b(B1) into save b(B2) increment DECVAL check range lowest byte lowest byte byte byte byte byte into save b(B0) into save b(B1) into save b(B2)
send DECVAL
next table entry
VALBUF+B0,WREG PutDec
unit position value VALBUF
#endif NAME: doError DESCRIPTION: Calculates position velocity error. doError
1997 Microchip Technology Inc. DS00532C-page
0291 0292 0293 0294 0295 0296
6A58 4A7C 6A59 4A7D 6A5A 4A7E
0297 0298 0299 029A 029B 029C
6A75 057C 6A76 037D 6A77 037E
029D 029E 029F 029F 02A0 02A1 02A2 02A3 02A4 02A5 02A6 02A7 02A8 02A9 02AA 02AA 02AB 02AC 02AD 02AE 02AF 02B0 02B1 02B2 02B3 02B4
9F7E C2AA 6A7D B580 097E 290A 327E C2B4 297E B07F 4A7D 2B7C C2B4 6A7D B37F 0B7E 2B0A 307E C2B4 2B7E 297D 877D 297C
01452 01453 01454 01455 01456 01457 ppos 01458 01459 01460 01461 01462 01463 01464 01465 01466 01467 01468 01469 pneg 01470 01471 01472 01473 01474 01475 01476 01477 01478 01479 01480 psatok 01481 01482
MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF SUB24 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB
POSITION,POSERROR POSITION+B0,WREG WREG,POSERROR+B0 POSITION+B1,WREG WREG,POSERROR+B1 POSITION+B2,WREG WREG,POSERROR+B2 MPOSITION,POSERROR MPOSITION+B0,WREG POSERROR+B0, MPOSITION+B1,WREG POSERROR+B1, MPOSITION+B2,WREG POSERROR+B2,
calculate position error byte into move b(B0) byte into move b(B1) byte into move b(B2)
lowest byte lowest byte byte byte byte byte
into save b(B0) into save b(B1) into save b(B2)
BTFSC GOTO MOVFP ANDLW IORWF CLRF CPFSGT GOTO CLRF MOVLW MOVPF SETF GOTO MOVFP IORLW ANDWF SETF CPFSLT GOTO SETF CLRF 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 lowest bits
clear high byte debug purposes
AN532
high byte 0xFF debug purposes
MOV24
VELOCITY,VELERROR
calculate velocity error
DS00532C-page 1997 Microchip Technology Inc.
AN532
02B4 02B5 02B6 02B7 02B8 02B9
6A5B 4A7F 6A5C 4A80 6A5D 4A81
02BA 02BB 02BC 02BD 02BE 02BF
6A78 057F 6A79 0380 6A7A 0381
02C0 02C1 02C2 02C2 02C3 02C4 02C5 02C6 02C7 02C8 02C9 02CA 02CB 02CC 02CD 02CD 02CE 02CF 02D0 02D1 02D2 02D3 02D4 02D5 02D6 02D7 02D7
9F81 C2CD 6A80 B580 0981 290A 3281 C2D7 2981 B07F 4A80 2B7F C2D7 6A80 B37F 0B81 2B0A 3081 C2D7 2B81 2980 8780 297F 0002
01483 01484 01485 01486 01487 01488 01489 01490 01491 01492 01493 01494 01495 01496 01497 01498 01499 01500 01501 01502 01503 01504 01505 01506 01507 01508 01509 01510 01511 01512 01513
MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF SUB24 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB
VELOCITY+B0,WREG WREG,VELERROR+B0 VELOCITY+B1,WREG WREG,VELERROR+B1 VELOCITY+B2,WREG WREG,VELERROR+B2 MVELOCITY,VELERROR MVELOCITY+B0,WREG VELERROR+B0, MVELOCITY+B1,WREG VELERROR+B1, MVELOCITY+B2,WREG VELERROR+B2,
byte into move b(B0) byte into move b(B1) byte into move b(B2)
lowest byte lowest byte byte byte byte byte
into save b(B0) into save b(B1) into save b(B2)
BTFSC GOTO vpos MOVFP ANDLW IORWF CLRF CPFSGT GOTO CLRF MOVLW MOVPF SETF GOTO vneg MOVFP IORLW ANDWF SETF CPFSLT GOTO SETF CLRF CLRF vsatok RETURN
VELERROR+B2,MSB vneg VELERROR+B1,WREG 0x80 VELERROR+B2, WREG, VELERROR+B2 vsatok VELERROR+B2, 0x7F WREG,VELERROR+B1 VELERROR, vsatok VELERROR+B1,WREG 0x7F VELERROR+B2, WREG, VELERROR+B2 vsatok VELERROR+B2, VELERROR+B1, VELERROR+B1,MSB VELERROR,
saturate error lowest bits
1997 Microchip Technology Inc. DS00532C-page
02D8
02D8 02D9 02DA 02DB
6A7C 4A8F 6A7D 4A90
02DC 02DD 02DE 02DF
7C8F 7D90 7E26 7F27
02E0 E630
02E1 02E2 02E3 02E4
5883 5984 5A85 5B86
02E5 290A 02E6 329A 02E7 E618
02E8 02E9 02EA 02EB
7C9B 7D9C 7E2A 7F2B
02EC E630
02ED 6A18
01514 01515 01516 01517 01518 01519 01520 01521 01522 01523 01524 01525 01526 01527 01528 01529 01530 01531 01532 01533 01534
NAME: doServo DESCRIPTION: Performs servo loop calculations. doServo MOV16 MOVFP MOVPF MOVFP MOVPF POSERROR,U0 POSERROR+B0,WREG WREG,U0+B0 POSERROR+B1,WREG WREG,U0+B1 save position error byte into move b(B0) byte into move b(B1)
LOADAB MOVFP MOVFP MOVFP MOVFP CALL MVPF32 MOVPF MOVPF MOVPF MOVPF
U0,KP U0+B0,AARG+B0 U0+B1,AARG+B1 KP+B0,BARG+B0 KP+B1,BARG+B1 Dmult DPX,Y DPX+B0,Y+B0 DPX+B1,Y+B1 DPX+B2,Y+B2 DPX+B3,Y+B3
compute KP*U0 load load load load byte byte byte byte AARG AARG BARG BARG
Y=KP*U0 move move move move A(B0) A(B1) A(B2) A(B3) B(B0) B(B1) B(B2) B(B3)
CLRF CPFSGT CALL LOADAB MOVFP MOVFP MOVFP MOVFP CALL ADD32 MOVFP
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
previous output saturated, accumulate integrator
compute KI*INTEGRAL load load load load byte byte byte byte AARG AARG BARG BARG
AN532
Y=KP*U0+KI*INTEGRAL lowest byte into
DS00532C-page 1997 Microchip Technology Inc.
AN532
02EE 02EF 02F0 02F1 02F2 02F3 02F4
0F83 6A19 1184 6A1A 1185 6A1B 1186
02F5 7C8F 02F6 7D90
02F7 02F8 02F9 02FA
6A91 051C 6A92 031D
02FB 7E28 02FC 7F29 02FD E630
02FE 02FF 0300 0301 0302 0303 0304 0305
6A18 0F83 6A19 1184 6A1A 1185 6A1B 1186
0306 0307 0308 0309 030A
290A 32C4 C311 78C4
030A 8804
ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC 01535 01536 MVFP16 MOVFP MOVFP 01537 SUB16 MOVFP SUBWF MOVFP SUBWFB 01538 MVFP16 MOVFP MOVFP 01539 CALL 01540 ADD32 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC MOVFP ADDWFC 01541 01542 CLRF 01543 CPFSGT 01544 GOTO 01545 MOVFP 01546 grabloop 01547 RLC32
Y+B0, DPX+B1,WREG Y+B1, DPX+B2,WREG Y+B2, DPX+B3,WREG Y+B3,
lowest byte byte byte byte byte byte byte
save into save into save into save
b(B0) b(B1) b(B2) b(B3)
U0,AARG U0+B0,AARG+B0 U0+B1,AARG+B1 U1,AARG U1+B0,WREG AARG+B0, U1+B1,WREG AARG+B1, 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,
compute KV*(U0-U1) move A(B0) B(B0) move A(B1) B(B1)
lowest byte lowest byte byte byte
into save b(B0) into save b(B1)
move A(B0) B(B0) move A(B1) B(B1)
Y=KP*U0+KI*INTEGRAL+KV*(U0-U1) lowest byte lowest byte byte byte byte byte byte byte into save b(B0) into save b(B1) into save b(B2) into save b(B3)
WREG, SHIFTNUM grabok SHIFTNUM,TMP _carry
1997 Microchip Technology Inc. DS00532C-page
030B 030C 030D 030E
1B83 1B84 1B85 1B86
030F 1718 0310 C30A 0311 0311 0312 0313 0314 0314 0315 0316 0317 0318 0319 031A 031B 031C 031D 031E 031F 0320 0321 0321 0322 0323 0324 0325 0326 0327 0328 0329 032A 032B 032C 032D
299A 9F86 C321 6A85 B580 0986 290A 3286 C32D 159A 2986 B07F 4A85 2B84 2B83 C32D 6A85 B37F 0B86 2B0A 3086 C32D 2B9A 2B86 2985 8785 2984 2983
032D 6A84 032E 4A87
01548 01549 01550 01551 01552 01553 01554 01555 01556 01557 01558 01559 01560 01561 01562 01563 01564 01565 01566 01567 01568 01569 01570 01571 01572 01573 01574 01575 01576 01577 01578 01579 01580 01581 01582 01583 01584 01585 01586
RLCF RLCF RLCF RLCF DECFSZ GOTO grabok CLRF BTFSC GOTO poss MOVFP ANDLW IORWF CLRF CPFSGT GOTO INCF CLRF MOVLW MOVPF SETF SETF GOTO negs MOVFP IORLW ANDWF SETF CPFSLT GOTO SETF SETF CLRF CLRF CLRF zero6bits MOV24 MOVFP MOVPF
Y+B0, Y+B1, Y+B2, Y+B3,
TMP, grabloop
SATFLAG, Y+B3,MSB negs Y+B2,WREG 0x80 Y+B3, WREG, Y+B3 zero6bits SATFLAG, Y+B3, 0x7F WREG,Y+B2 Y+B1, Y+B0, zero6bits Y+B2,WREG 0x7F Y+B3, WREG, Y+B3 zero6bits SATFLAG, Y+B3, Y+B2, Y+B2,MSB Y+B1, Y+B0,
saturate middle bits, keeping bits PW1DCH PW1DCL check 2**23
not, zero bits Y=0x007FFFFF clear debug purposes
check -2**23
not, zero bits 0xFF800000
AN532
Y+B1,YPWM+B0 Y+B1+B0,WREG WREG,YPWM+B0+B0
move YPWM zero bits byte into move b(B0)
DS00532C-page 1997 Microchip Technology Inc.
AN532
032F 0330 0331 0332
6A85 4A88 6A86 4A89
0333 0333 B0C0 0334 0B87 0335 0336 0337 0337 0338 0339 033A 033B 033C 9F88 C33E 9697 C344 2987 2988 2989 298A
033D C344 033E 033E 9597 033F C344 0340 0341 0342 0343 0344 0344 0345 0346 0347 2987 2988 2989 298A
B07F 4A19 B0C0 4A18
0348 0349 034A 034B
6A18 0F87 6A19 1188
034C 2919 034D B040 034E 4A18
01587 01588 01589 01590 01591 01592 01593 01594 01595 01596 01597 01598 01599 01600 01601 01602 01603 01604 01605 01606 01607 01608 01609 01610 01611 01612
MOVFP MOVPF MOVFP MOVPF doTorque MOVLW ANDWF BTFSC GOTO tplimit BTFSS GOTO CLR32 CLRF CLRF CLRF CLRF GOTO tmlimit BTFSS GOTO CLR32 CLRF CLRF CLRF CLRF mplimitok MOVLW MOVPF MOVLW MOVPF ADD16 MOVFP ADDWF MOVFP ADDWFC
Y+B1+B1,WREG WREG,YPWM+B0+B1 Y+B1+B2,WREG WREG,YPWM+B0+B2
byte into move b(B1) byte into move b(B2)
entry point torque mode 0xC0 YPWM+B0, YPWM+B1,MSB tmlimit EXTSTAT,BIT6 mplimitok YPWM YPWM+B0, YPWM+B1, YPWM+B2, YPWM+B3, mplimitok EXTSTAT,BIT5 mplimitok YPWM YPWM+B0, YPWM+B1, YPWM+B2, YPWM+B3,
PW1DCH_INIT WREG,TMP+B1 PW1DCL_INIT WREG,TMP+B0 TMP,YPWM TMP+B0,WREG YPWM+B0, TMP+B1,WREG YPWM+B1,
adjustment from bipolar unipolar duty cycle
lowest byte lowest byte byte byte
into save b(B0) into save b(B1)
CLRF MOVLW MOVPF ADD16
TMP+B1, 0x40 WREG,TMP+B0 TMP,YPWM
correct bit5 PW1DCL
1997 Microchip Technology Inc. DS00532C-page
034F 0350 0351 0352
6A18 0F87 6A19 1188
0353 0353 291A 0354 2989 0355 298A
0356 788D 0357 798E
0358 0359 035A 035B 035C 035D
6A87 0518 6A88 0319 6A89 031A
035E 971A 035F C365
0360 0361 0362 0363 0364 0365 0365 0366 0367
6A8D 4A87 6A8E 4A88 C376 291A 2989 298A
0368 788B 0369 798C
036A 6A87
MOVFP ADDWF MOVFP ADDWFC 01613 01614 testmax 01615 CLRF 01616 CLRF 01617 CLRF 01618 MVFP16 MOVFP MOVFP 01619 SUB24 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB 01620 BTFSS 01621 GOTO 01622 MOV16 MOVFP MOVPF MOVFP MOVPF 01623 GOTO 01624 testmin 01625 CLRF 01626 CLRF 01627 CLRF 01628 MVFP16 MOVFP MOVFP 01629 SUB24 MOVFP
TMP+B0,WREG YPWM+B0, TMP+B1,WREG YPWM+B1,
lowest byte lowest byte byte byte
into save b(B0) into save b(B1)
TMP+B2, YPWM+B2, YPWM+B3, YPWMAX,TMP YPWMAX+B0,TMP+B0 YPWMAX+B1,TMP+B1 YPWM,TMP YPWM+B0,WREG TMP+B0, YPWM+B1,WREG TMP+B1, YPWM+B2,WREG TMP+B2, TMP+B2,MSB testmin YPWMAX,YPWM YPWMAX+B0,WREG WREG,YPWM+B0 YPWMAX+B1,WREG WREG,YPWM+B1 limitok TMP+B2, YPWM+B2, YPWM+B3, YPWMIN,TMP YPWMIN+B0,TMP+B0 YPWMIN+B1,TMP+B1 YPWM,TMP YPWM+B0,WREG
check maximum limit LMD18200 must have minimum pulse duty cycle must 100%
move A(B0) B(B0) move A(B1) B(B1)
lowest byte lowest byte byte byte byte byte
into save b(B0) into save b(B1) into save b(B2)
saturate byte into move b(B0) byte into move b(B1)
check minimum limit
AN532
move A(B0) B(B0) move A(B1) B(B1)
lowest byte into
DS00532C-page 1997 Microchip Technology Inc.
AN532
036B 036C 036D 036E 036F
0518 6A88 0319 6A89 031A
0370 9F1A 0371 C376
0372 0373 0374 0375
6A8B 4A87 6A8C 4A88
0376 0376 B803 0377 7087 0378 7288
0379 037A 037B 037C
6A8F 4A91 6A90 4A92
037D 0002
037E
037E 037F 0380 0381 0382 0383
6A5E 4A62 6A5F 4A63 6A60 4A64
01630 01631 01632 01633 01634 01635 01636 01637 01638 01639 01640 01641 01642 01643 01644 01645 01646 01647 01648 01649 01650 01651
SUBWF MOVFP SUBWFB MOVFP SUBWFB BTFSC GOTO MOV16 MOVFP MOVPF MOVFP MOVPF limitok MOVLB MOVFP MOVFP MOV16 MOVFP MOVPF MOVFP MOVPF
TMP+B0, YPWM+B1,WREG TMP+B1, YPWM+B2,WREG TMP+B2, TMP+B2,MSB limitok YPWMIN,YPWM YPWMIN+B0,WREG WREG,YPWM+B0 YPWMIN+B1,WREG WREG,YPWM+B1
lowest byte byte byte byte byte
save b(B0) into save b(B1) into save b(B2)
saturate byte into move b(B0) byte into move b(B1)
BANK3 YPWM+B0,PW1DCL YPWM+B1,PW1DCH U0,U1 U0+B0,WREG WREG,U1+B0 U0+B1,WREG WREG,U1+B1
duty cycle
push errors into U(k-1) byte into move b(B0) byte into move b(B1)
RETURN NAME: doPreMove DESCRIPTION: doPreMove MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF NMOVVAL,MOVVAL NMOVVAL+B0,WREG WREG,MOVVAL+B0 NMOVVAL+B1,WREG WREG,MOVVAL+B1 NMOVVAL+B2,WREG WREG,MOVVAL+B2 move buffer MOVVAL byte into move b(B0) byte into move b(B1) byte into move b(B2)
1997 Microchip Technology Inc. DS00532C-page
0384 0385 0386 0387 0388
8F98 8698 8598 6ACB 4A99
0389 2954
038A 038B 038C 038D 038E 038F
6A58 4A55 6A59 4A56 6A5A 4A57
0390 0391 0392 0393 0394 0395 0396 0397
6A54 4AAF 6A55 4AB0 6A56 4AB1 6A57 4AB2
0398 299A 0399 296A 039A 296B
039B 296D 039C 296E
039D 296F 039E 2970
039F 2971 03A0 2972
01652 01653 01654 01655 01656 01657 01658 01659 01660 01661 01662 01663 01664 01665 01666 01667
MOVFP MOVPF
MOVSTAT,BIT7 MOVSTAT,BIT6 MOVSTAT,BIT5 ONE,WREG WREG,MOVFLAG
clear buffer flag motion status flag move progress flag initialize MOVEFLAG
CLRF MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOV32 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF CLRF CLR16 CLRF CLRF CLR16 CLRF CLRF CLR16 CLRF CLRF CLR16 CLRF CLRF CLR32
OPOSITION+B0, POSITION,OPOSITION+B1 POSITION+B0,WREG WREG,OPOSITION+B1+B0 POSITION+B1,WREG WREG,OPOSITION+B1+B1 POSITION+B2,WREG WREG,OPOSITION+B1+B2 OPOSITION,MOVPBUF OPOSITION+B0,WREG WREG,MOVPBUF+B0 OPOSITION+B1,WREG WREG,MOVPBUF+B1 OPOSITION+B2,WREG WREG,MOVPBUF+B2 OPOSITION+B3,WREG WREG,MOVPBUF+B3 SATFLAG, MOVTIME MOVTIME+B0, MOVTIME+B1, T1+B0, T1+B1, T2+B0, T2+B1, TAU+B0, TAU+B1, MOVDEL
initialize buffers
byte into move b(B0) byte into move b(B1) byte into move b(B2)
byte move b(B0) byte move b(B1) byte move b(B2) byte move b(B3)
into into into into
clear move times
used flag maximum speed
AN532
clear move discretization error
DS00532C-page 1997 Microchip Technology Inc.
AN532
03A1 03A2 03A3 03A4
29BB 29BC 29BD 29BE
03A5 29BF 03A6 29C0
03A7 3396 03A8 C404 03A9
03A9 7862 03AA 7963 03AB 7A64 03AC 971A 03AD C3B5
03AE 03AF 03B0 03B1 03B2 03B3 03B4 03B5 03B5 03B6 03B7 03B8 03B9
1318 1319 131A 290A 1518 1119 111A
291C 291D 291E 801C 811C
03BA 03BB 03BC 03BD 03BE 03BF
6A1C 0518 6A1D 0319 6A1E 031A
01668 01669 01670 01671 01672 pmode 01673 01674 01675 01676 01677 mvpos 01678 01679 01680 01681 01682 01683 01684
CLRF CLRF CLRF CLRF CLR16 CLRF CLRF
MOVDEL+B0, MOVDEL+B1, MOVDEL+B2, MOVDEL+B3,
clear phase flat counter
PH2FLAT PH2FLAT+B0, PH2FLAT+B1,
TSTFSZ GOTO MVFP24 MOVFP MOVFP MOVFP BTFSS GOTO NEG24 COMF COMF COMF CLRF INCF ADDWFC ADDWFC
MODETYPE vmode MOVVAL,TMP MOVVAL+B0,TMP+B0 MOVVAL+B1,TMP+B1 MOVVAL+B2,TMP+B2 TMP+B2,MSB mvpos TMP+B0, TMP+B1, TMP+B2, WREG, TMP+B0, TMP+B1, TMP+B2, move A(B0) B(B0) move A(B1) B(B1) move A(B2) B(B2)
CLRF CLRF CLRF SUB24 MOVFP SUBWF MOVFP SUBWFB MOVFP SUBWFB
MOVTMP+B0, MOVTMP+B1, MOVTMP+B2, MOVTMP+B0,BIT0 MOVTMP+B0,BIT1 MOVTMP,TMP MOVTMP+B0,WREG TMP+B0, MOVTMP+B1,WREG TMP+B1, MOVTMP+B2,WREG TMP+B2,
calculate abs(MOVVAL) immediate move negative
lowest byte lowest byte byte byte byte byte
into save b(B0) into save b(B1) into save b(B2)
1997 Microchip Technology Inc. DS00532C-page
03C0 03C1 03C2 03C3 03C4 03C5
971A C3CD 2B95 2999 8D98 8E98
03C6 03C7 03C8 03C9 03CA 03CB
6A62 0F58 6A63 1159 6A64 115A
03CC 0002 03CD 03CD 03CE 03CF 03D0 29B3 29B4 29B5 29B6
03D1 6A64 03D2 B580 03D3 4A6C 03D4 29AE
03D5 03D6 03D7 03D8 03D9 03DA
6A20 4AAB 6A21 4AAC 6A22 4AAD
03DB 29AA
03DC 03DD 03DE 03DF 03E0 03E1
6A23 4AA7 6A24 4AA8 6A25 4AA9
01685 BTFSS 01686 GOTO 01687 SETF 01688 CLRF 01689 01690 01691 ADD24 MOVFP ADDWF MOVFP ADDWFC MOVFP ADDWFC 01692 RETURN 01693 nonzero 01694 CLR32 CLRF CLRF CLRF CLRF 01695 01696 MOVFP 01697 ANDLW 01698 MOVPF 01699 01700 CLRF 01701 MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF 01702 CLRF 01703 MOV24 MOVFP MOVPF MOVFP MOVPF MOVFP MOVPF
TMP+B2,MSB nonzero SERVOFLAG, MOVFLAG, MOVSTAT,BIT5 MOVSTAT,BIT6 MOVVAL,POSITION MOVVAL+B0,WREG POSITION+B0, MOVVAL+B1,WR
Other recent searches
VPA2000 - VPA2000 VPA2000 Datasheet
TS1117B - TS1117B TS1117B Datasheet
KPT-3216SYC - KPT-3216SYC KPT-3216SYC Datasheet
IMH3A - IMH3A IMH3A Datasheet
EE-SX1107 - EE-SX1107 EE-SX1107 Datasheet
1108 - 1108 1108 Datasheet
1109 - 1109 1109 Datasheet
1131 - 1131 1131 Datasheet
EE-SX1131 - EE-SX1131 EE-SX1131 Datasheet
EA-1013-12A - EA-1013-12A EA-1013-12A Datasheet
DNF1300-T0700 - DNF1300-T0700 DNF1300-T0700 Datasheet
CS5207-1 - CS5207-1 CS5207-1 Datasheet
CS5207 - CS5207 CS5207 Datasheet
Privacy Policy | Disclaimer
© 2012 Datasheet Archive
