drivera.asm

步进微步控制（采用了１０细分的方式）很好实现了步进的细分控制

;
;    Source files for the project
;	drivera.asm	The main file
;	long.inc	32 bit header file				       
;								       
;    Schematic overview:					       
;	The design has 4 motor contollers.  There are two per axis       
;	with each axis packed into one 8 bit word.  Access is via      
;	register B and an 8 bit latch per axis.  Selects for the	       
;	latches are in register C.				       
;
;	A pot between +12 and ground is tied to an analog in and can   
;	be used to monitor the battery voltage. Two limit switches      
;	are implemented to allow the RA movement to be monitored.      
;
;	The internal RS232 module is expected to be used in async      
;	mode and is routed to a MAX222.				       
;
;    General Strategy:						       
;	The software implements a command loop that monitors the       
;	RS232 port for characters in polled mode.
;	The port is set up at 9600 baud, 8 bits, no parity
;	Characters are echoed and buffered to a full line terminated by a CR.
;	The line is parsed and executed.  All motor commands are kicked    
;	off but the timing is handled in interrupt commnands.	       
;	Given the pain of doing math on this system, the acceleration  
;	and speeds are currently compiled in.
;
;    Memory layout:
;	The first 0x800 is all interrupt code and interrupt support routines.
;	The second 0x800 is all user code and user level support routines.
;	A slow, goofy macro package is used to implement a user level stack
;	in the 2nd bank of registers.  The first bank is used for interrupt
;	processing.  The user level code is restricted to one deep stack
;	usage to allow the max. speed at interrupt level.
;
;    Timers:
;	Timer 1 is set to be free-running, clocked at 1/4 FOSC = 1MHZ
;	CCPR1 is used to time RA events 		       
;	CCPR2 is used to time Dec events
;	The timer is never shut off; match interrupts are turned on and off
;	as the drives need attention
;
;	Timer 0 is used as a free running clock, clocked at 1MHZ.  The prescaler is
;	set to 256 so that the timer rolls over every 65.536 MS or a little more
;	than 15 times per second.  It is used for gross user level timing of events
;	such as polling the battery value.
;
;    Directions
;	+ advances in terms of RA or Dec, ie, East or North
;	- decrements RA or DEC, ie, West or South
;	Tracking is inherently movement to the West and so is negative.
;
;    Command set						       
;	F<m><dir> <count>	Full step <count> steps		       
;	GC			Gets the count for both axes
;	GB			Gets the atod value and the rolling average value    
;	M<m><dir>		Starts an axis slewing       
;	Q<m>			Stop
;	I<m>			Idle: Quit and turn power off				       
;	R			Reset				       
;	SC<m> <count>		Sets the saved count for an axis
;	SS<speed>		Sets speed for next Slew command (both axes)
;	T			Starts RA tracking
;	U<m><dir> <count>	Microstep <count> steps		       
;
;	Abbreviations:						       
;	<count> - signed 32 bit count of full steps		       
;	<m> can be "R" or "D" and specifies which motor to use	       
;	<speed> is "T" for Tracking speed
;		   "G" for Guide speed
;		   "L" for Lunar
;		   "S" for Solar
;		   "M" for Medium
;		   "F" for Fast
;	<dir> is "+" or "-"
		page
; Main assembly options                           
		list	p=16c73b	; list directive to define processor
		radix   dec
		#include <p16c73b.inc>	; processor specific variable definitions
		nolist
		#include "long.inc" 	; Long (32 bit) arithmetic package header
		__CONFIG	_CP_OFF & _WDT_OFF & _BODEN_ON & _PWRTE_ON & _XT_OSC
;
; Macros
;
; User level subroutine call 
; 	Semantically same as call but takes more bytes and trashes z in status
;	Uses a stack starting in bank 2 at the top
;	Dest is the address of the routine to call
;
mi		set	0
mcall		macro	dest		;
mi		set	mi+1		;
		movwf	b0mwsave	; Stash w in bank 0 save
		movlw	high mcall#v(mi)
		movwf	PCLATH		; Means sub must be in this 2k page
		movlw	low mcall#v(mi)
		call	savertn		; Save return addr
		goto	dest		; and go to subroutine
mcall#v(mi)
		banksel	0
		movfw	b0mwsave	; Set up by mreturn
		endm
;
; User level subroutine return
; 	Converse of the mcall routine
;
mreturn		macro
		goto	restrtn		; Pop stack and return
		endm
;
; Long version of mreturn
;
mlreturn	macro
		lgoto	restrtn		; Pop stack and return
		endm
;
; User level subroutine return
;	Converse of mcall routine
; 	Semantially equivalent to retlw
;
mretlw		macro	rvalue		;
		movlw	rvalue		;
		goto	restrtn		;
		endm
;
; Long version of mretlw
;
mlretlw		macro	rvalue		;
		movlw	rvalue		;
		lgoto	restrtn		;
		endm
;
; Used to set contents of w into the ra control latch
;
ra_out		macro
		movwf	PORTB		; Stuff value into port B
		bsf	PORTC, RENABLE	; Set the RA latch bit	
		bcf	PORTC, RENABLE	; now clear it
		endm			;
;
; Used to set the contents of w into the dec control latch
;
dec_out		macro
		movwf	PORTB		; Stuff value into Port B
		bsf	PORTC, DENABLE	; Set the dec latch bit	
		bcf	PORTC, DENABLE	; now clear it
		endm
;
; RA Step
; 	w = direction, amount.  See FSTEPF, et al.
;	trashes w, temp
;
rstep		macro
		addwf	rtrajp, f	; see where to go next
		movlw	0x1f		; Wrap pointer
		andwf	rtrajp, w	; w gets ptr for gettraj
		movwf	rtrajp		; Save new ptr
		call	gettraj		; Get the table value
		ra_out			; Send to motor
		endm			;			
;
; Dec Step
; 	w = direction, amount.  See FSTEPF, et al.
;	trashes w, temp
;
dstep		macro
		addwf	dtrajp, f	; see where to go next
		movlw	0x1f		; Wrap pointer
		andwf	dtrajp, w	; w gets ptr for gettraj
		movwf	dtrajp		; Save new ptr
		call	gettraj		; Get the table value
		dec_out			; Send to motor
		endm			;
;
; Used to toggle the state of the LED
;
toggle_led	macro
		btfsc	PORTC, LED	;
		goto	$+3		;
		bsf	PORTC, LED	; Set the LED
		goto	$+2
		bcf	PORTC, LED	; Clear it instead		
		endm
;
; Old style unix internals macro names for stopping, allowing interrupts
;    Screwing around with interrupts disabled will ruin your timing
;    Don't use spl0 in interrupt routines, retfie will do it
;
splh		macro
		bcf	INTCON, GIE	;
		endm
;
spl0		macro
		bsf	INTCON, GIE	;
		endm
		list

;
; Global definitions
; Debug
; define SIM
;
#define ClkFreq 4000000			; input clock frequency = 4 MHz
#define baud(X) ((10*ClkFreq/(16*X))+5)/10 - 1
; Low speed tracking constants
#ifdef SIM
#define TRACKH	2	
#else
#define	TRACKH  high 	10000
#endif
#define TRACKL	low 	10000
#define	GUIDEBH	high	13000
#define	GUIDEBL	low	13000
#define	GUIDEFH	high	 7000
#define	GUIDEFL	low	 7000
#define LUNARH	high	 9651
#define	LUNARL	low	 9651
#define	SOLARH	high	10028
#define	SOLARL	low	10028
; High speed indexes (8x, 32x) into table
#define	MEDIDX	4
#define HIGHIDX	64
;
; Global equates
;
CMDLEN		equ	30		; 30 bytes in cmdbuff
MINBATT		equ	144		; Limit for battery ATOD value (10V)
STARTBATT	equ	173		; 12V starting value
;
; Arguments to rstep and dstep.  These depend on the traj table format
USTEPF		equ	1		; microstep forward
USTEPB		equ	-1		; microstep back
HSTEPF		equ	4		; half step forward
HSTEPB		equ	-4		; half step back
FSTEPF		equ	8		; full step forward
FSTEPB		equ	-8		; full step back
;
; Bits for rstatus, dstatus
;
ST_STOP		equ	0		; Stop issued
ST_STOPPED	equ	1		; Stopped
;
; Bits for slew
;
SL_FAST		equ	0		; Fast slew
SL_FAST_MASK	equ	0x01		;
SL_MED		equ	1		; Medium slew
SL_MED_MASK	equ	0x02		;
SL_GUIDE	equ	2		; Guide
SL_GUIDE_MASK	equ	0x04		;
SL_SLOW		equ	3		; Any of Track, lunar, solar
SL_SLOW_MASK	equ	0x08		;
;
; Port A
BATT		equ	0		; RA0 - Analog input to the pot attached to +12
LIM1		equ	2		; RA2 - Digital input: Limit switch on RA, moving West	
LIM2		equ	3		; RA3 - Digital input: Limit switch on RA, moving East
;
; Port C
LED		equ	2		; RC2 - Output: LED
LEDMASK		equ	0x04		; Mask for LED
SHDN		equ	3		; RC3 - Output: SHDN on MAX222
DENABLE		equ	4		; RC4 - Output: Enable Dec. motors latch
RENABLE		equ	5		; RC5 - Output: Enable RA motors latch
;
; RS-232
TXIF		equ	4		; Transmit buffer is empty if set
RCIF		equ	5		; Receive buffer is full if set
;
; Variables use in interrupt context saving
; 
w_temp		EQU	0x20		; variable used for context saving 
status_temp	EQU     w_temp+1	; variable used for context saving
temp_temp	EQU	status_temp+1	; save temp
temp1_temp	EQU	temp_temp+1	; Save temp1
pclath_temp	EQU	temp1_temp+1	; save pclath
int_end		EQU	pclath_temp+1	; Place holder for next interrupt vbl
;
; General globals
;
rapos		equ	int_end		; Long word to hold the ra count
dpos		equ	rapos+4		; Long word to hold the dec count
rtrajp		equ	dpos+4		; Trajectory pointer for ra drive
dtrajp		equ	rtrajp+1	; Trajectory pointer for dec drive
rstatus		equ	dtrajp+1	; Status for RA drive (Between slew cmd & interrupts)
dstatus		equ	rstatus+1	; Status for Dec drive
rinc		equ	dstatus+1	; Increment direction for RA - int level
dinc		equ	rinc+1		; Increment direction for Dec - int level
rspdhigh	equ	dinc+1		; RA speed high byte for int. routines
rspdlow		equ	rspdhigh+1	; RA speed low byte for int. routines
dspdhigh	equ	rspdlow+1	; Dec speed high byte for int. routines
dspdlow		equ	dspdhigh+1	; Dec speed low byte for int. routines
rac_cnt		equ	dspdlow+1	; Accel count passed back from getaccel
dac_cnt		equ	rac_cnt+1	; Accel count passed back from getaccel
stepcmd		equ	dac_cnt+1	; Motor step command -  command level
inccmd		equ	stepcmd+1	; Increment direction - command level
t1stepcmd	equ	inccmd+1	; Motor step command - t1 interrupt level
t2stepcmd	equ	t1stepcmd+1	; Motor step command - t2 interrupt level
slew		equ	t2stepcmd+1	; Communicates between set and slew cmds
temp		equ	slew+1		; Temp register to be used local to any routine
temp1		equ	temp+1		; Another temp
temp2		equ	temp1+1		; And another temp
temp3		equ	temp2+1		; Yet another temp
speedh		equ	temp3+1		; High byte of speed decoded in set speed cmd (or index)
speedl		equ	speedh+1	; Low byte of speed decoded in set speed cmd
rcuridx		equ	speedl+1	; RA Current acceleration index (interrupt level)
dcuridx		equ	rcuridx+1	; Dec Current acceleration index
rhiacidx	equ	dcuridx+1	; RA high acceleration index (interrupt level)
dhiacidx	equ	rhiacidx+1	; Dec high acceleration index (interrupt level)
t1state		equ	dhiacidx+1	; State byte for T1 CCPR1 interrupt
t2state		equ	t1state+1	; State byte for T1 CCPR2 interrupt
d0		equ	t2state+1	; 4 byte registers for fp routines
d1		equ	d0+4		;
d2		equ	d1+4		;
b0mwsave	equ	d2+4		; Bank 0 save for w reg - mcall
avgbatt		equ	b0mwsave+1	; Saves the average battery value
timbatt		equ	avgbatt+1	; Last time the battery was checked
t0ticks		equ	timbatt+1	; Incremented on each tick of t0
cmdidx		equ	t0ticks+1	; Index into cmdbuff
cmdbuff		equ	cmdidx+1	; Buffer for commands
gbl_end		equ	cmdbuff+CMDLEN	; End of globals
;
; Bank 2 stuff - msave, mreturn
;
mlsave		equ	H'00FF'		; Top register in 2nd bank - holds rtn
mfsave		equ	mlsave-1	; Save for FSR
stackptr	equ	mfsave-1	; Save for stack pointer
stack		equ	stackptr-1	; Bottom of stack, grows down
;
; Simulator stuff
	ifdef	SIM
fakeidx		equ	gbl_end		;
	endif
;	
;
; Startup code
;                                                                 
		ORG	0x000		; processor reset vector
		clrf    PCLATH		; ensure page bits are claared
		lgoto    main		; go to beginning of program
;
; Interrupt routines
;
		ORG     0x004		; interrupt vector location
		movwf   w_temp		; save off current W register contents
		movf	STATUS,w	; move status register into W register
		bcf     STATUS,RP0	; ensure file register bank set to 0
		movwf	status_temp	; save off contents of STATUS register
		movfw	temp		; Save temp
		movwf	temp_temp	;
		movfw	temp1		;
		movwf	temp1_temp	;
		movfw	PCLATH		; Save PCLATH
		movwf	pclath_temp	;
; prolog end
		clrf	PCLATH		; Force bank 0
		btfss	PIR1, 2		; Check for timer 1 compare (CCP1IF) match first
		goto	int1		;
; t1 interrupt
t1int
		bcf	PIR1, 2		; Clear CCP1IF
		movlw	high t1jmptbl	;
		movwf	PCLATH		;
		movfw	t1state		; Pick up jump vector
		addwf	PCL, f		; Go
t1jmptbl
		goto	t1stepxof8	; 0 Step 2
		goto	t1stepxof8	; 1 Step 3
		goto	t1stepxof8	; 2 Step 4
		goto	t1stepxof8	; 3 Step 5
		goto	t1stepxof8	; 4 Step 6
		goto	t1stepxof8	; 5 Step 7
		goto	t1step8of8	; 6 Step 8
		goto	t1step1of8	; 7 Step 1
		goto	t1accelxof8	; 8 Step 2
		goto	t1accelxof8	; 9 Step 3
		goto	t1accelxof8	; 10 Step 4
		goto	t1accelxof8	; 11 Step 5
		goto	t1accelxof8	; 12 Step 6
		goto	t1accelxof8	; 13 Step 7
		goto	t1accel8of8	; 14 Step 8
		goto	t1accel1of8	; 15 Step 1
		goto	t1hsxof8	; 16 Step 1
		goto	t1hsxof8	; 17 Step 2
		goto	t1hsxof8	; 18 Step 3
		goto	t1hsxof8	; 19 Step 4
		goto	t1hsxof8	; 20 Step 5
		goto	t1hsxof8	; 21 Step 6
		goto	t1hsxof8	; 22 Step 7
		goto	t1hs8of8	; 23 Step 8
		goto	t1decel1of8	; 24 Step 1
		goto	t1decelxof8	; 25 Step 2
		goto	t1decelxof8	; 26 Step 3
		goto	t1decelxof8	; 27 Step 4
		goto	t1decelxof8	; 28 Step 5
		goto	t1decelxof8	; 29 Step 6
		goto	t1decelxof8	; 30 Step 7
		goto	t1decel8of8	; 31 Step 8
t1stepxof8
		movfw	t1stepcmd	;
		rstep			; Step RA Axis
		incf	t1state, f	;
		goto	t1intend	;
t1step8of8
		movfw	t1stepcmd	; 8th step
		rstep			;
		incf	t1state, f	; Set to step 1
		movfw	rinc		; increment
		addwd	rapos		; Advance rapos - 8 microsteps done
		toggle_led		; Flip LED - full step done
		goto	t1intend	;
t1step1of8
		btfss	rstatus, ST_STOP ; Do they want to stop?
		goto	t1step1of8a	; Skip over if not
t1wrap ; Code to shut off timer 1
		clrf	rstatus		; OK, let's stop...
		bsf	rstatus, ST_STOPPED ;
		movlw	B'00000000'	; CCP1X:CCP1Y=0 CCP1M3:CCP1M0=0
		movwf	CCP1CON		; Shut off compare on match
		banksel	PIE1		; 
		bcf	PIE1, 2		; Turn off CCP1IE
		banksel	0		;
		goto	int1		; Process rest of interrupts
t1step1of8a	movfw	t1stepcmd	;
		rstep			; 1st of 8 microsteps
		movlw	0		; State 0 next
		movwf	t1state		;
		goto	t1intend	;
t1accelxof8	movfw	t1stepcmd	; Step it
		rstep			;
		incf	t1state, f	; Advance nicely along...
		goto	t1intend	;
t1accel8of8
		movfw	t1stepcmd	; 8th step
		rstep			;
		incf	t1state, f	; Set to step 1
		movfw	rinc		; Account for a full step
		addwd	rapos		;
		toggle_led		; We were here
		btfsc	rstatus, ST_STOP ; Stop before we got there?
		goto	t1accel8of8b	;
		decfsz	rac_cnt, f	; More steps at this speed?
		goto	t1intend	; Go on if not
; Accelerating, check next step
		movfw	rcuridx		;
		subwf	rhiacidx, w	; At top speed?
		skpnz			;
		goto	t1accel8of8a	; change state if yes
		incf	rcuridx, f	; Point at next one
		movfw	rcuridx	;
		call	rgetaccel	; Reload CCPRx, rac_cnt
		goto	t1intend	; wrap it up
t1accel8of8a	movlw	16		; State t1hs1of8 next
		movwf	t1state		;
		goto	t1intend	; Wrap it up
t1accel8of8b ; Wants to stop before we hit full speed
		movfw	rcuridx		; Where are we?
		skpnz			;
		goto	t1intend	; Pick it up in t1accel1of8 if done
		decf	rcuridx, f	; Head back down if not
		movfw	rcuridx		;
		call	rgetaccel	; Load "down 1" index
		movlw	24		; decel state - t1decel1of8
		movwf	t1state		;
		goto	t1intend	;
t1accel1of8
		btfss	rstatus, ST_STOP ; Do they want to stop?
		goto	t1accel1of8a	; Skip over if not
		movfw	rcuridx		; Probably 0 if we are here (see t1accel8of8b)
		skpnz			; Wrap it up if yes
		goto	t1wrap		; Will see it in t1accel8of8 then decel if not
t1accel1of8a	movfw	t1stepcmd	;
		rstep			; 1st of 8 microsteps
		movlw	8		; State 8 - t1accelxof8 next
		movwf	t1state		;
		goto	t1intend	;
t1hsxof8
		movfw	t1stepcmd	; Step it
		rstep			;
		incf	t1state, f	; Next state
		goto	t1intend	;
t1hs8of8
		movfw	t1stepcmd	; 8th step
		rstep			;
		movlw	16		; Set to t1hsxof8
		movwf	t1state		;
		movfw	rinc		; Account for a full step
		addwd	rapos		;
		toggle_led		; We were here
;	ifdef	SIM