; *********************************************
;*
;* 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 02:25, local time: 2024/11/17 02:15,
18.217.195.183: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? <A HREF="http://massmind.org/techref/microchip/io/dev/motor/servob/SERVOB.ASM"> microchip io dev motor servob SERVOB</A> |
| Did you find what you needed? |
Welcome to massmind.org! |
Welcome to massmind.org! |
.