please dont rip this site

Microchip IO Dev Motor Servob Servob.asm

; *********************************************
;*
;*  PIC based PM motor drive controller
;*  Pioneer Microsystems, Inc.
;*  Pittsburgh, Pa.        (412)-369-9920
;*  Christopher Eddy, PE, 4/1/97
;*
;*********************************************
;*
;*  Board is based on 16C73A.
;*
;**********************************************
;* configuration switches
;*
        TITLE   "PM motor speed controller code, rev b by cle/pu"
        ; chris eddy, 4/1/97
        list    N=66,C=120,p=16c73A
;        __CONFIG _WDT_ON & _CP_OFF & _HS_OSC & _PWRTE_ON
        include "p16c73.inc"

FALSE           	SET     0
TRUE            	SET     1

;******************************************************************
;*************** DETERMINE OVERCURRENT CUTOUT *************
; 1/4V=1A, 5V=FS, FS=20A. FOR 10A USE 128 DIGITAL
;******************************************************************
#DEFINE		OVERCURRENT	D'128'

;******************************************************************
;*************** DECIDE IF WE SHOULD RUN PRE-EMPHASIS *************
;******************************************************************
; note, also known as IR compensation.
#DEFINE		PREEMPHASIS	FALSE

;******************************************************************
;*************** DECIDE IF WE SHOULD RUN DEADBAND *************
;******************************************************************
#DEFINE		DEADBAND_ENABLE	FALSE

;******************************************************************
;*************** SET THE PWM FREQUENCY *************
;******************************************************************
#DEFINE		PWM_FREQUENCY	D'163'
;#DEFINE		PWM_FREQUENCY	D'97'	;100Hz
;#DEFINE		PWM_FREQUENCY	D'163'	;60Hz
;#DEFINE		PWM_FREQUENCY	D'255'	;38Hz

;******************************************************************
;*************** DETERMINE ANALOG DEADBAND *************
;******************************************************************
#DEFINE		DEADBAND	D'10'

;*************** PORT DIRECTION SETUP CONSTANTS **********************
PORTASETUP		equ	B'11111111'
PORTBSETUP		equ	B'11111111'
PORTCSETUP		equ	B'00000001'

;******************************************************************
;***************DECLARE VARIABLES****************************
;******************************************************************
MOTOR_DROP		equ	H'20'
TEMPORARY               equ     H'21'
DIFERROR                equ     H'22'
PROPORTION              equ     H'23'
INTEGRAL                equ     H'24'
ATODCHAN                equ     H'25'
VOLTAGE_INPUT           equ     H'26'
CURRENT_INPUT           equ     H'27'
SUPPLY_INPUT            equ     H'28'
COMMAND_INPUT           equ     H'29'
RAMP_SPEED		EQU	H'2A'
UTILITY                 equ     H'2B'
ACCaLO                  EQU     H'2C'
ACCaHI                  EQU     H'2D'
ACCbLO                  EQU     H'2E'
ACCbHI                  EQU     H'2F'
ACCcLO                  EQU     H'30'
ACCcHI                  EQU     H'31'
ACCdLO                  EQU     H'32'
ACCdHI                  EQU     H'33'
temp			equ     H'34'
sign			equ     H'35'
RAMP_TIME		equ     H'36'
TEMPORARY2		equ     H'37'
ADWAIT			equ     H'38'
POT_VR1			equ     H'39'
POT_VR2			equ     H'3A'
POT_VR3			equ     H'3B'
INTEGRAL_TIME		equ     H'3C'
MY_PWM			equ	H'3D'
MY_TARGET		equ	H'3E'
CURRENT_TRIP		equ	H'3F'
AVERAGE_LO              equ     H'40'
AVERAGE_HI              equ     H'41'
VOLTAGE_POINTER         equ     H'42'
CURRENT_POINTER         equ     H'43'
SUPPLY_POINTER          equ     H'44'
COMMAND_POINTER         equ     H'45'
PWM_DIFF		equ     H'46'
HOWFAROFF		equ     H'47'

SIGNED			equ     FALSE	; Set This To 'TRUE' for signed math

;******* UPPER PAGE VARIABLES used for a digital filter *****************
VOLTAGE_FILTER          equ     H'D8' ; TO DF
CURRENT_FILTER          equ     H'E0'
SUPPLY_FILTER           equ     H'E8'
COMMAND_FILTER          equ     H'F0'

;******************************************************************
;******* INTERRUPT STORAGE BYTES COVER BOTH PAGES ***********************
;******************************************************************
TEMP_W                  EQU     H'7C'  ; AND FC
TEMP_FSR                EQU     H'7D'  ; AND FD
TEMP_STAT               EQU     H'7E'  ; AND FE
TEMP_PCLATH             EQU     H'7F'  ; AND FF

;******************************************************************
;*************** PIC I/O PINS (A PORT) *************************************
;******************************************************************
; see analog input code

;******************************************************************
;*************** PIC I/O PINS (B PORT) *************************************
;******************************************************************
SW0			equ     0       ; bit 0, in
SW1			equ     1       ; bit 1, in
SW2			equ     2       ; bit 2, in
SW3			equ     3       ; bit 3, in

;******************************************************************
;*************** PIC I/O PINS (C PORT) *************************************
;******************************************************************
INHIBIT			equ     0       ; bit 0, INPUT
;DIRECT			equ     1       ; bit 1, output
PWM			equ     2       ; bit 2, output
TESTING			equ     3       ; bit 3, output
;_DIRECT		equ     4       ; bit 4, output
ADJ1			equ     5       ; bit 5, output
ADJ2			equ     6       ; bit 6, output
ADJ3			equ     7       ; bit 7, output

;******************************************************************
;*************** UTILITY FLAGS **********************
;******************************************************************
CARRYFLAG               equ     0
INTEGRAL_NOW		equ     1
DELAYED_ANALOG_START	EQU     2
SIGN_OF_ERROR		EQU     3
P_CONTROL		equ     4
I_CONTROL		equ     5
IV_CONTROL		equ     6
STOP_CONTROL		equ     7

;******************************************************************
;*************** CODE POINT DEFINITIONS **********************
;******************************************************************
RESET                   equ     H'0000'
INTERRUPT               equ     H'0004'
CODEP0                  equ     H'0005'
CODEP1                  equ     H'0800'

;*************** MAIN PROGRAM DECLARATIONS *******************
; FOLLOWING CODE IS IN PAGE 0, IN VECTORS AREA
;******************************************************************
        org     RESET           ;RESET VECTOR
	goto    event_START
        goto    event_START
        goto    event_START
        goto    event_START
        org     INTERRUPT       ;INTERRUPT VECTOR
;*************** INTERUPT SERVICE ROUTINE **************************
; INTERRUPT IS SERVICED BY THE GOTO IN THE ADDRESS VECTOR AT ADDRESS 4.
; interrupts can be caused by the following sources:
; timer0, ad complete, iic,
; in our case, they will be caused by;
; if int on ad capture, get ana conversion, place in mem
; TIMER0 is a timed tick that allows key input monitoring et al.
;******************************************************************
INTERRUPT_SERVICE 
        ; PUSH REGISTERS SEE PG.82
        MOVWF   TEMP_W
        SWAPF   STATUS,W
        BCF     STATUS,RP0
        MOVWF   TEMP_STAT
        MOVF    PCLATH,W
        MOVWF   TEMP_PCLATH
        MOVF    FSR,W
        MOVWF   TEMP_FSR
        ; CHECK FOR A TIMER 0 OVERFLOW
        ; ONLY CHECK IF INT IS ENABLED
        BTFSS   INTCON,T0IE     
        GOTO    INT_1
        BTFSS   INTCON,T0IF        
	GOTO	INT_1
	CALL    TIMERTICK
INT_1        
        ; CHECK THE PERIPHERAL INTERRUPTS
        ; ONLY CHECK IF INT IS ENABLED
        BTFSS   INTCON,PEIE
        GOTO    INT_2
        ; CHECK FOR A TIMER 2 OVERFLOW
        ; ONLY CHECK IF INT IS ENABLED
	BSF	STATUS,RP0
        BTFSS   PIE1,TMR2IE     
        GOTO    INT_3
	BCF	STATUS,RP0
        BTFSS   PIR1,TMR2IF        
	GOTO	INT_3
	CALL    TIMER2TICK
INT_3        
        ; ONLY CHECK A/D COMPLETE IF INT IS ENABLED
	BSF	STATUS,RP0
        BTFSS   PIE1,ADIE
        GOTO    INT_2
	BCF	STATUS,RP0
        BTFSC   PIR1,ADIF
        CALL    GETAD
INT_2        
        ; POP REGISTERS SEE PG.82
        BCF     STATUS,RP0
        MOVF    TEMP_FSR,W
        MOVWF   FSR
        MOVF    TEMP_PCLATH,W
        MOVWF   PCLATH
        SWAPF   TEMP_STAT,W
        MOVWF   STATUS
        SWAPF   TEMP_W,F
        SWAPF   TEMP_W,W
INTDNE  RETFIE

;*************** CODE PAGE 0 START ********************************
; FOLLOWING CODE IS IN PAGE 0
;******************************************************************
;        org     CODEP0

	include	"maths.inc"

event_START   
        BCF     STATUS,RP0
        MOVLW   H'00'
        MOVWF   PORTA
        BSF     STATUS,RP0
        MOVLW   PORTASETUP
        MOVWF   TRISA
        BCF     STATUS,RP0
        MOVLW   H'00'
        MOVWF   PORTB
        BSF     STATUS,RP0
        MOVLW   PORTBSETUP
        MOVWF   TRISB
        BCF     STATUS,RP0
        MOVLW   H'00'
        MOVWF   PORTC
        BSF     STATUS,RP0
        MOVLW   PORTCSETUP
        MOVWF   TRISC
        ; option register
        ; bit 7 ~rbpu SET TO 0 SO PULLUP IN PORT B READ WORKS
        ; bit 6 INTEDG.. program as a one
        ; bit 5 T0CS 1=RC3 PIN, 0=INT CLK FOR TIM0
        ; bit 4 T0SE 1=HI TO LOW, 0= OPPOSITE EDGE TIM0
        ; bit 3 PSA assigned to WDT, set to 1
        ; bit 2 to 0 are prescale setting
        ; code  wdt    tmr0
        ;  000  1:1     1:2
        ;  001  1:2     1:4
        ;  010  1:4     1:8
        ;  011  1:8     1:16
        ;  100  1:16    1:32
        ;  101  1:32    1:64
        ;  110  1:64    1:128
        ;  111  1:128   1:256
        ; example WDT, internal clock prescale of 128
        ; 18mS*128 = 2.304 seconds
        BSF     STATUS,RP0
        MOVLW   B'01000010'
        MOVWF   OPTION_REG      ;SET WDT PRESCALER & PULL UPS
        ; CLEAR THE PIR1 BEFORE ENABLING INTERRUPTS
        ; PIR1 bit definitions, clear before enabling
        ; bit 7 PSPIF: PAR SLAVE PORT interrupt status
        ; bit 6 ADIF: A/D CONV COMPLETE INT 
        ; bit 5 RCIF: SERIAL RX INT FLAG
        ; bit 4 TXIF: SERIAL TX INT FLAG
        ; bit 3 SSPIF: SYNC PORT int status
        ; bit 2 CCP1IF: CAPTURE/COMPARE/PWM INT
        ; bit 1 TMR2IF: TIMER 2 INT FLAG
        ; bit 0 TMR1IF: TIMER 1 INT FLAG
        BCF     STATUS,RP0
        MOVLW   B'00000000'
        MOVWF   PIR1
        ; T1CON bit definitions
        ; bit 7 NO DEF
        ; bit 6 NO DEF 
        ; bit 5 T1CKPS1: PRESCALE BITS
        ; bit 4 T1CKPS0: 0=1,01=2,10=4,11=8
        ; bit 3 T1OSCEN: 1=OSC EN, 0=OSC DIS
        ; bit 2 ~T1SYNC: IGNORED FOR INT CLOCK
        ; bit 1 TMR1CS: 1=EXT, 0=INT CLK
        ; bit 0 TMR1ON: 1=ENABLE 0=DISABLE TIMER 1
        ; LEAVE TIMER OFF UNTIL SETUP IS PERFORMED
        BCF     STATUS,RP0
        MOVLW   B'00000000'
        MOVWF   T1CON
        ; T2CON bit definitions
        ; bit 7 NO DEF
        ; bit 6 TOUTPS3: \ 
        ; bit 5 TOUTPS2:  \  POST SCALE SELECT, SCALE 1 TO 16
        ; bit 4 TOUTPS1:  /
        ; bit 3 TOUTPS0: /
        ; bit 2 TMR2ON: 1=ON, 0=OFF
        ; bit 1 T2CKPS1: \  CLOCK PRESCALER, 00=1, 01=4, 1x=16
        ; bit 0 T2CKPS0: /
        BCF     STATUS,RP0
        MOVLW   B'00000100'
        MOVWF   T2CON
        ; CCP1CON bit definitions
        ; bit 7 NO DEF
        ; bit 6 NO DEF
        ; bit 5 CCP1X: \  HIGH TWO BITS ALLOW 10 BIT RESOLUTION ON PWM
        ; bit 4 CCP1Y: /
        ; bit 3 TOUTPS3: \ 
        ; bit 2 TOUTPS2:  \  MODE, SEE BOOK
        ; bit 1 TOUTPS1:  /  11XX MAKES PWM MODE
        ; bit 0 TOUTPS0: /
        BCF     STATUS,RP0
        MOVLW   B'00000000'
        MOVWF   CCP1CON
        ; PIE1 bit definitions
        ; bit 7 PSPIE: PAR SLAVE PORT interrupt status
        ; bit 6 ADIE: A/D CONV COMPLETE INT 
        ; bit 5 RCIE: SERIAL RX INT ENABLE
        ; bit 4 TXIE: SERIAL TX INT ENABLE
        ; bit 3 SSPIE: SYNC PORT int status
        ; bit 2 CCP1IE: CAPTURE/COMPARE/PWM INT
        ; bit 1 TMR2IE: TIMER 2 INT FLAG
        ; bit 0 TMR1IE: TIMER 1 INT FLAG
        BSF     STATUS,RP0
        MOVLW	B'01000010'
        MOVWF   PIE1
        ; interrupt control register as such;
        ; bit 7 GIE Global interrupt enable
        ; bit 6 PEIE peripheral int enable
        ; bit 5 T0IE tmr0 int enable
        ; bit 4 INTE INTF ENABLE BIT
        ; bit 3 RBIE ENABLE INT ON PORT B0:3 CHANGE
        ; bit 2 T0IF tmr0 overflow prog as any
        ; bit 1 INTF EXTERNAL INTERRUPT FLAG
        ; bit 0 RBIF FLAG ON PORT B0:3 CHANGE
        BCF     STATUS,RP0
        movlw   B'01100000'
        movwf   INTCON
        ; ADCON0 control register as such;
        ; bit 7 ADCS1 \
        ; bit 6 ADCS0 -A/D freq select
        ;               00=fosc/2
        ;               01=fosc/8
        ;               10=fosc/32
        ;               11=rc oscillator
        ; bit 5 CHS2  \
        ; bit 4 CHS1  -select channel
        ; bit 3 CHS0  /
        ; bit 2 GO/_DONE conversion progress 1=running, 0=complete
        ; bit 1 reserved
        ; bit 0 A/D operate 1=on
        ; note: do not set bit 0 and 2 in the same instruction
        BCF     STATUS,RP0
        MOVLW   B'11000001'
        MOVWF   ADCON0
        ; ADCON1 control register as such;
        ; bit 7 reserved
        ; bit 6 reserved
        ; bit 5 reserved
        ; bit 4 reserved
        ; bit 3 reserved
        ; bit 2 PCFG2 \
        ; bit 1 PCFG1 -A/D pin configuration control
        ; bit 0 PCFG0 /
        ; see book for combinations
        ; 101 sets AN0 and AN1 with ref on AN3
        ; 000 sets ALL with ref FROM VCC
        BSF     STATUS,RP0
        MOVLW   B'00000000'
        MOVWF   ADCON1
        ; RCSTA control register as such;
        ; bit 7 SPEN SERIAL PORT ENABLE
        ; bit 6 RC8/9 1=9 0=8 BIT RECIEVE
        ; bit 5 SREN DONT CARE IN ASYNCH
        ; bit 4 CREN 1=ENABLE RECIEVE, 0=DISABLE RECIEVE
        ; bit 3 NOT IN USE
        ; bit 2 FERR FRAMING ERROR BIT, 1= ERROR
        ; bit 1 OERR OVERRUN ERROR BIT 1=OVERRUN
        ; bit 0 RCD8 9TH BIT OF DATA, CAN BE PARITY
        BCF     STATUS,RP0
        MOVLW   B'00000000'
        MOVWF   RCSTA
        ; TXSTA control register as such;
        ; bit 7 CSRC CLOCK SOURCE DONT CARE IN ASYNCH
        ; bit 6 TX8/9 1=9 BIT TX, 0=8 BIT
        ; bit 5 TXEN 1= ENABLE TRANSMIT
        ; bit 4 SYNC 1=SYNCHRONOUS MODE, 0=ASYNCHRONOUS
        ; bit 3 UNIMPLEMENTED
        ; bit 2 BRGH 1=HIGH SPEED, 0=LOW SPEED (HIGH SPEED RIFE WITH BUGS)
        ; bit 1 TRMT 1=TX REG EMPTY, 0= FULL
        ; bit 0 TXD8 9TH BIT OF DATA, CAN BE PARITY
        BSF     STATUS,RP0
        MOVLW   B'00000000'
        MOVWF   TXSTA
        ; CLEAR UTILITY FLAGS
        BCF     STATUS,RP0
        clrf    UTILITY
        clrf    ATODCHAN
	BSF	STATUS,RP0
	MOVLW	PWM_FREQUENCY
	MOVWF	PR2
	BCF	STATUS,RP0
        ; SETUP INTERRUPTS TO FUNCTION
	BSF	INTCON,GIE

;******************************************************************
event_BEGIN
;******************************************************************
        CLRWDT

;******************************************************************
; configure I/O ports again so we don't lose port directions
;******************************************************************
event_PORT_DIRN
        BSF     STATUS,RP0
        MOVLW   PORTASETUP
        MOVWF   TRISA
        MOVLW   PORTBSETUP
        MOVWF   TRISB
        MOVLW   PORTCSETUP
        MOVWF   TRISC
        BCF     STATUS,RP0

;******************************************************************
; GET B PORT AND SORT INTO STATE FLAGS..
;******************************************************************
	BTFSS	PORTB,SW0
        BSF	UTILITY,P_CONTROL
	BTFSC	PORTB,SW0
        BCF	UTILITY,P_CONTROL
	BTFSS	PORTB,SW1
        BSF	UTILITY,I_CONTROL
	BTFSC	PORTB,SW1
        BCF	UTILITY,I_CONTROL
	BTFSS	PORTB,SW2
        BSF	UTILITY,IV_CONTROL
	BTFSC	PORTB,SW2
        BCF	UTILITY,IV_CONTROL
        ; IF NEITHER P NOR I NOR IV OR INHIBIT, STOP
CLEARINT
	BCF	INTCON,GIE
	BTFSC	INTCON,GIE
	GOTO	CLEARINT
        BSF	UTILITY,STOP_CONTROL
        BTFSC	PORTC,INHIBIT
	GOTO	STATE01
        BTFSC	UTILITY,P_CONTROL
	BCF	UTILITY,STOP_CONTROL
        BTFSC	UTILITY,I_CONTROL
	BCF	UTILITY,STOP_CONTROL
        BTFSC	UTILITY,IV_CONTROL
	BCF	UTILITY,STOP_CONTROL
STATE01
	BSF	INTCON,GIE
	
	GOTO	event_BEGIN	

;******************************************************
;           GET FINISHED A/D CONVERSION
;******************************************************
GETAD  
        bcf     STATUS,RP0
        bcf     PIR1,ADIF       ;clear flag
        ; FLAG THE RESULTING CONVERSION FOR PROCESSING
        btfss   ATODCHAN,0      ;chan 0 (voltage)
	GOTO	GET01
	; PROCESS THE VOLTAGE READING
	; PLACE VALUE IN FILTER
	INCF	VOLTAGE_POINTER,F
	; IF HIGH, CLEAR
	BTFSC	VOLTAGE_POINTER,3
	CLRF	VOLTAGE_POINTER
        ; NOW PUT IN MEM
	MOVLW	VOLTAGE_FILTER
	ADDWF	VOLTAGE_POINTER,W
	MOVWF	FSR
	MOVFW	ADRES
	MOVWF	VOLTAGE_INPUT
	MOVFW	ADRES
	MOVWF	INDF
	; AVERAGE THE VALUES IN MEMORY
;        MOVLW	VOLTAGE_FILTER
;	CALL	AVERAGE_8
	; MOVE RESULT INTO MAIN LOCATION
;	MOVWF	VOLTAGE_INPUT        
	GOTO    event_PROCESS_END
GET01
        btfss   ATODCHAN,1      ;chan 1 (current)
	GOTO	GET02
	; PROCESS THE CURRENT READING
	MOVFW	ADRES
	MOVWF	CURRENT_INPUT        
	GOTO    event_PROCESS_END
GET02
        btfss   ATODCHAN,2      ;chan 2 (supply)
	GOTO	GET03
	; PROCESS THE SUPPLY READING
	; PLACE VALUE IN FILTER
	INCF	SUPPLY_POINTER,F
	; IF HIGH, CLEAR
        BTFSC	SUPPLY_POINTER,3
        CLRF	SUPPLY_POINTER
        ; NOW PUT IN MEM
        MOVLW	SUPPLY_FILTER
        ADDWF	SUPPLY_POINTER,W
        MOVWF	FSR
	MOVFW	ADRES
;        MOVWF	INDF
	; AVERAGE THE VALUES IN MEMORY
;        MOVLW	SUPPLY_FILTER
;        CALL	AVERAGE_8
	; MOVE RESULT INTO MAIN LOCATION
	MOVWF	SUPPLY_INPUT        
	GOTO    event_PROCESS_END
GET03
        btfss   ATODCHAN,3      ;chan 3 (command)
	GOTO	GET04
	; PROCESS THE COMMAND READING
	; MUST SCALE THIS READING SO THAT 0-255 FROM A/D WILL
        ; BECOME 0-TARGET DELTA MOTOR READING
	; REMEMBER THAT SUPPLY READING FULL SCALE IS 0-80VDC
	; LATE ADDITION.. SCALE FURTHER FOR A SUB-SPAN,
	; SUCH THAT 0-63% REPRESENTS FULL SPEED (21VDC) (FEEDER)
	; AND 0-77% REPRESENTS FULL SPEED (42VDC) (SUPPLY)
	; RE-FORMULATED FOR 0-100% CMD,
	; FEEDER: 0-100% CMD = 21V*100%/63% OR 0-33.33VDC
	; SUPPLY: 0-100% CMD = 42V*100%/77% OR 0-54.54VDC
	; FEEDER: MUL BY 100, DIV BY 240.
	; SUPPLY: MUL BY 100, DIV BY 146.
	; BASE RANGE ON SWITCH 4.
	; BUT ALL OF THIS SUBRANGE CAN BE #DEFINED OUT SEE ABOVE
	MOVFW	ADRES
	; CALCULATE SCALED COMMAND AND MOVE RESULT INTO MAIN LOCATION
	movwf   ACCaLO
	movlw   0
	movwf   ACCaHI
	movlw   0
	movwf   ACCbHI
        MOVLW	D'100'
        MOVWF	ACCbLO
	call    D_mpyF
	; RESULT IN B AND C, LSB IN C, IS ALL WE WANT
        MOVFW	ACCcHI
        MOVWF	ACCbHI
        MOVFW	ACCcLO
        MOVWF	ACCbLO
        MOVLW	0
        MOVWF	ACCaHI
	BTFSS	PORTB,SW3
	movLW	D'240'
	BTFSC	PORTB,SW3
	movLW	D'146'
	movwf   ACCaLO
        CALL	D_divF
        MOVFW	ACCbLO
	MOVWF	COMMAND_INPUT

	IF	PREEMPHASIS==TRUE
	; if required, calculate a pre-emphasis on
	; the low end to compensate for weak
	; performance under load. So, if cmd<15%, and
	; current goes up, then boost the command setpoint.
	; seems that 15% command is 15/255 to final pwm.
GET03D
	MOVLW	D'15'	; ANYTHING BELOW 15%
	SUBWF	COMMAND_INPUT,W
	BTFSC	STATUS,C
	GOTO	GET03B
;	MOVLW	D'3'	; ANYTHING ABOVE 390mA
	; CURRENT TRIP.. 5 IS GOOD FOR 12-15%,
	; AND 3 IS GOOD FOR 7-12%.
	MOVFW	COMMAND_INPUT
	ADDLW	D'20'
;	MOVWF	CURRENT_TRIP
;	BCF	STATUS,C
;	RRF	CURRENT_TRIP,F
;	BCF	STATUS,C
;	RRF	CURRENT_TRIP,W
	SUBWF	CURRENT_INPUT,W
	BTFSS	STATUS,C
	GOTO	GET03B
	; INCREASE COMMAND BY 10%
;	MOVLW	D'3'
;	ADDWF	COMMAND_INPUT,F
	BCF	STATUS,C
	RLF	COMMAND_INPUT,F
GET03B
	ENDIF
	GOTO    event_PROCESS_END
GET04
        btfss   ATODCHAN,4      ;chan 4 (pot)
	GOTO	GET05
	COMF	ADRES,W
	MOVWF	POT_VR1
	GOTO    event_PROCESS_END
GET05
        btfss   ATODCHAN,5      ;chan 5 (pot)
	GOTO	GET06
	MOVFW	ADRES
	MOVWF	POT_VR2
	GOTO    event_PROCESS_END
GET06
        btfss   ATODCHAN,6      ;chan 6 (pot)
	GOTO	event_PROCESS_END
	MOVFW	ADRES
	MOVWF	POT_VR3
event_PROCESS_END

	; SET A FLAG FOR THE MAIN ROUTINE
	BSF	UTILITY,DELAYED_ANALOG_START
;******************************************************************
event_SETUPAD
;******************************************************************
	BTFSS	UTILITY,DELAYED_ANALOG_START
	GOTO	event_ADGET_end
	; setup the next A/D conversion
	; select the next channel
	bcf     STATUS,C
	rlf     ATODCHAN,F
	btfsS   ATODCHAN,7
        GOTO	SETUP_A
	BCF	UTILITY,DELAYED_ANALOG_START
	BCF	PORTC,TESTING
	movlw   h'01'
	movwf   ATODCHAN
        GOTO	event_ADGET_end
SETUP_A
	; if accidentally set to 0
	movfw   ATODCHAN
	btfsc   STATUS,Z
	incf    ATODCHAN,F
	; select the right channel in hardware
	btfsc   ATODCHAN,0      ;VOLTAGE
	MOVLW	B'10000001'
	btfsC   ATODCHAN,1      ;CURRENT
	MOVLW	B'10001001'
	btfsC   ATODCHAN,2      ;SUPPLY
	MOVLW	B'10010001'
	btfsC   ATODCHAN,3      ;COMMAND
	MOVLW	B'10011001'
	btfsC   ATODCHAN,4      ;POT VR1
	MOVLW	B'10100001'
	btfsC   ATODCHAN,5      ;POT VR2
	MOVLW	B'10100001'
	btfsC   ATODCHAN,6      ;POT VR3
	MOVLW	B'10100001'
	MOVWF	ADCON0
; SET THE POT MULTIPLEX
	BCF	PORTC,ADJ1
	BCF	PORTC,ADJ2
	BCF	PORTC,ADJ3
	btfsC   ATODCHAN,4      ;POT VR1
	BSF	PORTC,ADJ3
	btfsC   ATODCHAN,5      ;POT VR2
	BSF	PORTC,ADJ2
	btfsC   ATODCHAN,6      ;POT VR3
	BSF	PORTC,ADJ1
; MAKE SURE ONE IS SET
	BTFSC	PORTC,ADJ1
        GOTO	event_ADSET_end
        BTFSC	PORTC,ADJ2
        GOTO	event_ADSET_end
        BTFSC	PORTC,ADJ3
        GOTO	event_ADSET_end
        ; NONE SET.. SET ONE
        BSF	PORTC,ADJ3
event_ADSET_end	

;******************************************************************
; start a/d converter conversion
; must wait 15uS, then start a/d. @10mhZ, 38CYCLES.
;******************************************************************
	MOVLW	D'218'
        MOVWF	ADWAIT
WAITING_1
	INCFSZ	ADWAIT,F
	goto	WAITING_1
	bsf     ADCON0,GO_DONE
event_ADGET_end	

	RETURN

;******************************************************************
AVERAGE_8
;******************************************************************
; AVERAGE 8 VALUES AS A FILTER FUNCTION        
        MOVWF   FSR
	CLRF	TEMPORARY
        CLRF	AVERAGE_LO
        CLRF	AVERAGE_HI
AVERAGE_1
	MOVFW	INDF
        ADDWF	AVERAGE_LO,F
        BTFSC	STATUS,C
        INCF	AVERAGE_HI,F
        INCF	TEMPORARY,F
        BTFSS	TEMPORARY,3
        GOTO	AVERAGE_1
        RRF	AVERAGE_HI,F
        RRF	AVERAGE_LO,F
        RRF	AVERAGE_HI,F
        RRF	AVERAGE_LO,F
        RRF	AVERAGE_HI,F
        RRF	AVERAGE_LO,W
        return
        
;******************************************************************
;*************** TIMER SERVICE ROUTINE **************************
;******************************************************************
TIMERTICK
	; INT COMES FOSC/4/128/256 (WAS FOSC/4/8/256)
        ; FOR 10.00M, 13mS (WAS 820uS)
        BCF     STATUS,RP0
        BCF     INTCON,T0IF

; ADD AN OPTIONAL RAMP ACCELERATE CONTROL
; ( DECELERATE IS NO BIG DEAL )
; PRETEND WE LOOK AT THE RAMP POT TO MANAGE SPEED.
; FURTHER SAY TYPICAL RAMP ON 0-100D TRANSITION IS FULL
; TRANSIT IN 2 SECONDS.  SAY WE MAKE A TIMER HERE THAT
; ROLLS IN 13mS (WAS 802uS) * 16 (WAS 256) OR 200mS.
; MID SCALE ON POT IS 128.
; NEED 2000mS/200mS OR 10 INCREMENTS IN THAT TIME.
; 100 COUNTS / 10 INCREMENTS IS 10 UNITS, POT AT HALF IS
; 128, SHIFT DOWN THREE FOR 16 COUNT, SO WERE IN NEIGHBORHOOD.
	INCF	RAMP_TIME,F
	BTFSS	RAMP_TIME,6
	GOTO	RAMP_01
	CLRF	RAMP_TIME
	; 200mS VISIT.
	MOVFW	RAMP_SPEED
	SUBWF	COMMAND_INPUT,W
	BTFSS	STATUS,C
	GOTO	RAMP_02
	; DIF WAS POSITIVE, COMM>RAMP
	MOVWF	TEMPORARY
	MOVFW	POT_VR1
	MOVWF	TEMPORARY2
	BCF	STATUS,C
	RRF	TEMPORARY2,F
	BCF	STATUS,C
	RRF	TEMPORARY2,F
	BCF	STATUS,C
	RRF	TEMPORARY2,F
	BCF	STATUS,C
	RRF	TEMPORARY2,F
	INCF	TEMPORARY2,F	; ZERO WOULD BE A DISASTER
	MOVFW	TEMPORARY2
	SUBWF	TEMPORARY,W
	; DID DIFF - MAX FROM POT
	; IF POS, LIMIT TO MAX FROM POT
	; IF NEG, WRITE COMMAND INTO RAMP
	BTFSS	STATUS,C
	GOTO	RAMP_02
	MOVFW	TEMPORARY2
	ADDWF	RAMP_SPEED,F
	GOTO	RAMP_01
RAMP_02
	; DIF WAS NEGATIVE, REPLACE RAMP WITH COMMAND
	MOVFW	COMMAND_INPUT
;	MOVFW	RAW_CMD
	MOVWF	RAMP_SPEED
RAMP_01

; INCREMENT INTEGRAL TIME FOR LATER
        BCF	UTILITY,INTEGRAL_NOW
	INCF	INTEGRAL_TIME,F
	MOVFW	INTEGRAL_TIME
        SUBWF	POT_VR2,W
        BTFSC	STATUS,C
        GOTO	INTEGRAL_NOW_END
        BSF	UTILITY,INTEGRAL_NOW
	CLRF	INTEGRAL_TIME
INTEGRAL_NOW_END

	; NEED TO KNOW SUPPLY MINUS VOLTAGE FOR MOTOR DROP
	MOVFW	VOLTAGE_INPUT
        SUBWF	SUPPLY_INPUT,W
        MOVWF	MOTOR_DROP
        ; MOTOR NEVER GOES NEGATIVE, IF SLIGHTLY OFF, CLEAR TO ZERO
	BTFSS	STATUS,C
        CLRF	MOTOR_DROP

	; CALCULATE ERROR MAGNITUDE AND SIGN
	MOVFW	MOTOR_DROP
        SUBWF	RAMP_SPEED,W
        MOVWF	DIFERROR
	BTFSC	STATUS,C
	BSF	UTILITY,SIGN_OF_ERROR
	BTFSS	STATUS,C
	BCF	UTILITY,SIGN_OF_ERROR

	; SPLIT BASED UPON SIGN OF THE ERROR
	BTFSS	UTILITY,SIGN_OF_ERROR
	GOTO	ACT_1
        ; GET HERE IF COMMAND-MOTOR DROP IS POSITIVE
        ; MUST INCREASE SPEED OF MOTOR
	; INTEGRAL COMPONENT
	BTFSS	UTILITY,INTEGRAL_NOW
	GOTO	ACT_1B
;	MOVFW	DIFERROR
;	MOVLW	D'1'
	BCF	STATUS,C
	RRF	DIFERROR,W
	ADDWF	INTEGRAL,F
	; IF CARRY, MAX OUT AT FF
	BTFSS	STATUS,C
	GOTO	ACT_1B
	MOVLW	H'FF'
	MOVWF	INTEGRAL
ACT_1B
	; PROP COMPONENT
	; CALCULATE SCALED ERROR AND MOVE RESULT INTO MAIN LOCATION
	MOVFW	DIFERROR
	movwf   ACCaLO
	movlw   0
	movwf   ACCaHI
	comf	POT_VR3,w	; pot is wired backwards
	movwf   ACCbLO
	bcf	STATUS,C
	rrf	ACCbLO,F	; div by two to spread pot
	bcf	STATUS,C
	rrf	ACCbLO,F	; div by two to spread pot
	movlw   0
	movwf   ACCbHI
	call    D_mpyF
	; RESULT IN B AND C, LSB IN Clo,
        ; IF MSB IS SET SEND FF
        MOVFW	ACCcLO
        MOVWF	PROPORTION
	MOVFW	ACCcHI
        BTFSC	STATUS,Z
        GOTO	ACT_2
        MOVLW	H'FF'
	MOVWF	PROPORTION
        GOTO	ACT_2
ACT_1	; GET HERE IF COMMAND-MOTOR DROP IS NEGATIVE
	; MUST DECREASE SPEED OF MOTOR
	; INTEGRAL COMPONENT
	BTFSS	UTILITY,INTEGRAL_NOW
        GOTO	ACT_1D
	MOVFW	DIFERROR
;	MOVLW	H'FE'
;	BSF	STATUS,C
;	RRF	DIFERROR,W
	ADDWF	INTEGRAL,F
	; IF CARRY, MIN DOWN TO 00
        BTFSC	STATUS,C
        GOTO	ACT_1D
        MOVLW	H'00'
        MOVWF	INTEGRAL
ACT_1D
	; PROP COMPONENT
	CLRF	PROPORTION
        GOTO	ACT_2
ACT_2
	; IF CMD IS ZERO, CLEAR INTEGRAL
	MOVFW	COMMAND_INPUT
        BTFSC	STATUS,Z
        CLRF	INTEGRAL

	; IF SET TO STOP, DO SO
        BTFSS	UTILITY,STOP_CONTROL
	GOTO	ACT_2B
        CLRF	TEMPORARY
	; ALSO SET THE RAMP_SPEED TO 0 TO ENABLE ACCEL
	CLRF	RAMP_SPEED
        GOTO	ACT_3
ACT_2B
	; IF SET TO P_CONTROL, DO SO
        BTFSS	UTILITY,P_CONTROL
	GOTO	ACT_2D
	MOVFW	PROPORTION
        MOVWF	TEMPORARY
        GOTO	ACT_3
ACT_2D
	; IF SET TO I_CONTROL, DO SO
        BTFSS	UTILITY,I_CONTROL
	GOTO	ACT_2E
	MOVFW	INTEGRAL
        MOVWF	TEMPORARY
        GOTO	ACT_3
ACT_2E
	; IF SET TO IV_CONTROL, DO SO
        BTFSS	UTILITY,IV_CONTROL
	GOTO	ACT_2F
	MOVLW	H'FF'
        MOVWF	TEMPORARY
	MOVFW	INTEGRAL
	ADDWF	PROPORTION,W
	BTFSS	STATUS,C
        MOVWF	TEMPORARY
        GOTO	ACT_3
ACT_2F
	; ERROR, JUST STOP
        CLRF	TEMPORARY
ACT_3
	; LAST SECOND ABORT IN CASE OF OVERCURRENT
	; CURRENT_INPUT IS SCALES 0-255=0-10A
	MOVLW	OVERCURRENT
	SUBWF	CURRENT_INPUT,W
	BTFSC	STATUS,C
	CLRF	TEMPORARY

	IF	DEADBAND_ENABLE==TRUE
	; IMPLEMENT A DEADBAND
        MOVFW	MY_TARGET
        MOVWF	PWM_DIFF
        MOVFW	TEMPORARY
        SUBWF	PWM_DIFF,F
        BTFSS	STATUS,C
        COMF	PWM_DIFF,F
	MOVFW	PWM_DIFF
        SUBLW	DEADBAND
        BTFSC	STATUS,C
        GOTO	ACT_END
;        MOVWF	HOWFAROFF
	ENDIF

	; SAVE TARGET INTO PWM TARGET
	MOVFW	TEMPORARY
	MOVWF	MY_TARGET
ACT_END

	RETURN

;******************************************************************
;*************** TIMER 2 SERVICE ROUTINE **************************
;******************************************************************
TIMER2TICK 
	; INT COMES FOSC/4/PR2
        ; FOR 10.00M & PR2=97, 39uS.
        BCF     STATUS,RP0
        BCF     PIR1,TMR2IF

	; CREATE MY OWN PWM
	INCF	MY_PWM,F

	MOVLW	D'240'
        SUBWF	MY_PWM,W
	BTFSS	STATUS,Z
        GOTO	TMR1_GO
	BSF	UTILITY,DELAYED_ANALOG_START
	BSF	PORTC,TESTING
	CALL	SETUP_A	; A CHEAT TO TRIP THE AD
TMR1_GO
	; IF CMD IS ZERO, KILL THE PWM
	MOVFW	COMMAND_INPUT
	BTFSS	STATUS,Z
	GOTO	TMR2_GO
	BSF	PORTC,PWM
	RETURN
TMR2_GO
	; IF STOP SET, KILL THE PWM
	BTFSS	UTILITY,STOP_CONTROL
	GOTO	TMR3_GO
	BSF	PORTC,PWM
	RETURN
TMR3_GO
	; NOT ZERO, GO AHEAD AND DO IT.
	MOVFW	MY_PWM
	SUBWF	MY_TARGET,W
	BTFSS	STATUS,C
	BSF	PORTC,PWM
	BTFSC	STATUS,C
	BCF	PORTC,PWM
	RETURN

        END




file: /Techref/microchip/io/dev/motor/servob/SERVOB.ASM, 28KB, , updated: 2001/4/6 03:25, local time: 2024/3/28 04:48,
TOP NEW HELP FIND: 
44.210.107.64:LOG IN

 ©2024 These pages are served without commercial sponsorship. (No popup ads, etc...).Bandwidth abuse increases hosting cost forcing sponsorship or shutdown. This server aggressively defends against automated copying for any reason including offline viewing, duplication, etc... Please respect this requirement and DO NOT RIP THIS SITE. Questions?
Please DO link to this page! Digg it! / MAKE!

<A HREF="http://www.sxlist.com/techref/microchip/io/dev/motor/servob/SERVOB.ASM"> microchip io dev motor servob SERVOB</A>

Did you find what you needed?

 

Welcome to sxlist.com!


Site supported by
sales, advertizing,
& kind contributors
just like you!

Please don't rip/copy
(here's why

Copies of the site on CD
are available at minimal cost.
 

Welcome to www.sxlist.com!

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

  .