Why PID when something simple will suffice.
I havent tried the A2D version yet.
If the A2D version doesnt work well then its PID or a class D type triangle wave and comparator technique.
It'll probably work alright at very low output currents. But once you have to replenish the charge drawn by the load on a more frequent basis, the output voltage will drop like a rock.
Some 12~13 years ago, I wrote a PID control routine for an 8051 uC. It took less than a screenful (25 lines) of assembly code to do it. You could probably do it in one line in C...
Recall that the derivative of the error signal, E, is merely E👎-E(n-1), where n is the sample number. The integral, I, is I👎 = I(n-1)+E👎. You'll have to limit the integral value to something reasonable to prevent the register from rolling over.
I'm not suggesting that you implement the full PID control, though, that would probably result in a pretty well-performing system. All I'm saying is to use proportional control. I.e. PWM_value = ADC_value * P. That's one line of code... In assembly, it might be three instructions. Plus whatever is needed to set up the PWM and ADC. If you limit P to powers of two, you can use a shift instruction to perform the multiplication. Recall X*2^n = X << n.
~Tom
I professionally wrote a PID program about 12 years ago.
It was for a big drill company.
It used 49*ERROR1-70*ERROR2+25*ERROR3 and scaled the result to get the right gain.
I did it in 16 bit arithmetic on a PIC.
It worked very well, the drill speed was rock steady.
It was just a matter of getting the gain right.
A colleague of mine got the formula from a PID seminar and it seemed to work with anything we threw at it with a little tweaking.
Just finished a discrete class d amplifier so into PWM at the moment.
I don't know about that, I in my tests I had no problems, I even slowed it down
With 16-bit math it takes longer to compute. Mine was 8-bit math and it actually worked quite well for controlling a small plotter.
~Tom
Still no big deal -- I don't recall which PICs have multipliers, but I think all AVR do. 8 bit hardware multiply makes even 16 bit multiplications speedy. When your loop bandwidth is a few kHz and the clock is over 1MHz, heck, you could still do it without hardware multiply.
Needless to say, you'd be using the analog comparator (an external one if the internal isn't fast / accurate enough) for a current-mode boost / flyback converter, and wrapping the loop around that.
Hmm, you need a DAC to do a proper current mode thing. At 1MHz clock, 1kHz bandwidth, you only get 10 bits PWM, and that's assuming a brick wall antialiasing filter. You only get ~7 bits for a factor of 10 frequency margin, and that's still a lot of ripple from a simple RC. Maybe sigma delta modulation would be better?
Tim
Needless to say, you'd be using the analog comparator (an external one if the internal isn't fast / accurate enough) for a current-mode boost / flyback converter, and wrapping the loop around that.
Hmm, you need a DAC to do a proper current mode thing. At 1MHz clock, 1kHz bandwidth, you only get 10 bits PWM, and that's assuming a brick wall antialiasing filter. You only get ~7 bits for a factor of 10 frequency margin, and that's still a lot of ripple from a simple RC. Maybe sigma delta modulation would be better?
Tim
The PIC seems to work ok with a simple pulse out to the SMPS if the voltage is too low.
However I decided to go the whole hog and do a PID version.
I just put in a read of the A2D to get an error number and put that into a PID I already had. It oscilated to start with because the gain was too high.
A slight reduction in gain saw a steady output voltage.
Altering the load made no difference to the voltage tracking.
Here is the main program.
I havent included two other files macro.asm and maths.asm
;**************************************************************************************
;MAIN PROGRAM
;**************************************************************************************
MLOOP
BSF TESTPIN
CALL READA2D ;GET CURRENT VOLTAGE
BCF TESTPIN
;SHIFT DOWN ERRORS
MOVF ERROR2,W
MOVWF ERROR3
MOVF ERROR1,W
MOVWF ERROR2
;NEW ERROR1
MOVLW 100
MOVWF TEMP
MOVF ADRES,W
SUBWF TEMP,W
MOVWF ERROR1
;****************************
CLR16 RESULT_0
;****************************
;CALC ERROR1 * K1
CLRF NO1_1 ;CLEAR HIGH BYTES
CLRF NO2_1
MOVLW K1
MOVWF NO1_0
MOVF ERROR1,W
MOVWF NO2_0
CALL SIGNEDMPY8
CALL ANSERTORESULT
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;ERROR2 * K2
CLR16 NO1_0
CLR16 NO2_0
MOVLW K2
MOVWF NO1_0
MOVF ERROR2,W
MOVWF NO2_0
CALL SIGNEDMPY8
;SUBTRACT ANSER FROM RESULT
CALL NEGANSERWORD ;NEGATE 2ND WORD THEN ADD
MOVLW RESULT_0
MOVWF FSR
MOVF ANSER_0,W
CALL ADDINCFSR
MOVF ANSER_1,W
CALL ADDWFC
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;ERROR3 * K3
CLR16 NO1_0
CLR16 NO2_0
MOVLW K3
MOVWF NO1_0
MOVF ERROR3,W
MOVWF NO2_0
CALL SIGNEDMPY8
;ADD ANSER TO RESULT
MOVLW RESULT_0
MOVWF FSR
MOVF ANSER_0,W
CALL ADDINCFSR
MOVF ANSER_1,W
CALL ADDWFC
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
CALL SCALERESULT ;THIS DECIDES GAIN OF SYSTEM
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;SUBTRACT RESULT FROM PHASEANGLE TO GET CORRECT POSITION
BTFSC RESULT_0,7
GOTO NEGCHANGE
;ADD RESULT TO PHASEANGLE CHECKING FOR OVERFLOW
POSCHANGE
MOVF RESULT_0,W
ADDWF PHASEANGLE,W
BC PAOVERFLOWED
MOVWF PHASEANGLE
GOTO PHASEANGLEDONE
PAOVERFLOWED MOVLW 255
MOVWF PHASEANGLE
GOTO PHASEANGLEDONE
NEGCHANGE MOVF RESULT_0,W
ADDWF PHASEANGLE,W
BNC PAUNDERFLOWED
MOVWF PHASEANGLE
GOTO PHASEANGLEDONE
PAUNDERFLOWED
CLRF PHASEANGLE
GOTO PHASEANGLEDONE
PHASEANGLEDONE
;*********************************
;*********************
;WAIT PHASEANGLE
;*********************
MOVF PHASEANGLE,W
MOVWF TEMP
BZ RET ;EXIT IF 0
BSF MOSFET
WP1_ DECFSZ TEMP,F
GOTO WP1_
RET BCF MOSFET
;;;;;;;;;;;;;;;;;;;;
GOTO MLOOP ;ENOUGH DELAY DOING MATHS TO DISCHARE IDUCTOR
The subroutines and macros are as named.
I am still working on it and when it is finished I will upload the final complete version.
Done a bit more testing.
With the circuit I have I can output anything upto 15 volts.
Altering the voltage divider will allow higher output voltages.
The PID seems to be pretty stable, a little noise on the output but the PID is working at 3KHz on a 8MHz PIC.
With the circuit I have I can output anything upto 15 volts.
Altering the voltage divider will allow higher output voltages.
The PID seems to be pretty stable, a little noise on the output but the PID is working at 3KHz on a 8MHz PIC.
Changed the code from 8 bit multiply to 16 bit.
This has allowed a bigger range of output voltages.
Found a little instabilty in the PID routine.
Tracked this down to a noisy signal on the input to the A2D converter.
A 100nf sorted that out.
current code for pid control.
;INTH0.ASM
;*********
;*****************************************************
;SMPS PID PROGRAM FOR GENERAL USE. (C) N.P.WRIGHT 2010
;*****************************************************
INCLUDE MACRO.ASM
LIST R=DEC
LIST P=PIC12F510
;INCLUDE REGISTER DEFINITIONS
INCLUDE P12F510.INC
;==========================================================================
;
; Configuration Bits
;
;==========================================================================
;_IOSCFS_ON EQU H'0FFF' 8MHZ
;_IOSCFS_OFF EQU H'0FDF' 4MHZ
;_MCLRE_ON EQU H'0FFF'
;_MCLRE_OFF EQU H'0FEF'
;_CP_ON EQU H'0FF7'
;_CP_OFF EQU H'0FFF'
;_WDT_ON EQU H'0FFF'
;_WDT_OFF EQU H'0FFB'
;_LP_OSC EQU H'0FFC'
;_XT_OSC EQU H'0FFD'
;_IntRC_OSC EQU H'0FFE' INTERNAL RC OSC
;_ExtRC_OSC EQU H'0FFF'
__CONFIG _MCLRE_OFF & _WDT_OFF & _CP_OFF & _IntRC_OSC & _IOSCFS_ON
;*****************************************
;SUBWF FLAGS
;IF W > MEM THEN NC
;IF W = MEM THEN C
;IF W < MEM THEN C
;IF W<= MEM THEN C
;IF MEM>=W THEN C
;IF MEM<W THEN NC
;*****************************************
;******************************************
FIRSTRAM EQU 10 ;USED FOR DEBUG PURPOSES
CBLOCK FIRSTRAM
TEMP
NO1_0 ;NO1 (USED FOR MATHS)
NO1_1
NO2_0 ;NO2 (USED FOR MATHS)
NO2_1
ANSER_0 ;ANSWER (USED FOR MATHS)
ANSER_1
SIGN ;USED IN MATHS
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
ERROR1_0 ;NEWEST ERROR
ERROR1_1
ERROR2_0 ;PREVIOUS ERROR
ERROR2_1
ERROR3_0 ;PREVIOUS PREVIOUS ERROR
ERROR3_1
RESULT_0 ;USED IN MAIN CALCULATION
RESULT_1
PHASEANGLE ;CURRENT PHASEANGLE
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
ENDC
#DEFINE CARRYFLAG STATUS,0
;*******************************************
FIRSTA EQU 0 ;FIRST DATA TO PORTA
STATEA EQU 4
#DEFINE TESTPIN GPIO,4
#DEFINE MOSFET GPIO,1
#DEFINE A2D GPIO,2
;CONSTANTS USED FOR PID CONTROLLER (DONT CHANGE)
K1 EQU 49
K2 EQU 70
K3 EQU 25
;*********************************************
ORG 0
RESET GOTO AFTERRESET
;********************************
INCLUDE MATHS.ASM ;MATHS FUNCTIONS
;*************************************************
NEGRESULT16 NEG16 RESULT_0
RETLW 0
;**************************************
;GET A2D VALUE TO ADRES
READA2D BSF ADCON0,1 ;GO
NOP
NOP
READLP BTFSC ADCON0,1
GOTO READLP
RETLW 0
;**************************************
;THIS ALTERS THE GAIN OF THE SYSTEM
;HIGH GAIN CAUSES OVERSHOOT
;LOW GAIN CAUSES UNDERSHOOT
;
SCALERESULT
;THIS FIRST PART SCALES FOR BEST PERFORMANCE
;ALTHOUGH THIS MIGHT STILL CAUSE OVERFLOWS IN EXCEPTIONAL CASES
RRSIGNED16 RESULT_0 ;SCALING FACTOR = 2^N (GAIN)
RRSIGNED16 RESULT_0
RRSIGNED16 RESULT_0
RRSIGNED16 RESULT_0
RRSIGNED16 RESULT_0
; RRSIGNED16 RESULT_0
; RRSIGNED16 RESULT_0
; RRSIGNED16 RESULT_0
;THIS PART FORCES THE ANSWER TO BE IN THE RANGE -127 TO +127
;THIS PART ONLY COMES IN AT EXTREMES OF RANGES AND CONDITIONS
MOVF RESULT_1,W
MOVWF SIGN ;SAVE SIGN TO B7 OF SIGN
BTFSC RESULT_1,7 ;MAKE +VE TO SIMPLY ROUTINE
CALL NEGRESULT16
FIX MOVLW 80H ;CHECK IF IN RANGE YET. 0-127 IS IN RANGE
ANDWF RESULT_0,W
IORWF RESULT_1,W
BZ FIXED ;YES SO GO
CLRC ;+VE DIV BY 2
RRF RESULT_1,F
RRF RESULT_0,F
GOTO FIX ;RETEST
FIXED BTFSC SIGN,7 ;PUT SIGN BACK
CALL NEGRESULT16
RETLW 0
;*************************************
AFTERRESET
MOVWF OSCCAL
CLRWDT
MOVLW 55H
MOVWF CM1CON0 ;DISABLE COMPARATOR ON GPIO,1!
;SETUP PIC I/O AND OPTIONS
MOVLW FIRSTA
MOVWF GPIO
MOVLW STATEA
TRIS GPIO
MOVLW 0C0H ;WEAK PULL UPS OFF & WAKE UP ON PIN CHANGE OFF
OPTION
;SETUP A2D CONVERTER
MOVLW 69H ;CLOCK DIV 8
MOVWF ADCON0
;CLEAR MEMORY. DONT USE FSR AS TOP 2 BITS ARE SET
CLRF ERROR1_0
CLRF ERROR1_1
CLRF ERROR2_0
CLRF ERROR2_1
CLRF ERROR3_0
CLRF ERROR3_1
CLRF PHASEANGLE
;**************************************************************************************
;MAIN PROGRAM
;**************************************************************************************
MLOOP
BSF TESTPIN ;LISTON2
CALL READA2D
BCF TESTPIN ;LISTON2
;SHIFT DOWN ERRORS
MOVF ERROR2_0,W ;ERROR 2 TO ERROR 3
MOVWF ERROR3_0
MOVF ERROR2_1,W
MOVWF ERROR3_1
MOVF ERROR1_0,W ;ERROR 1 TO ERROR 2
MOVWF ERROR2_0
MOVF ERROR1_1,W
MOVWF ERROR2_1
;GET NEW ERROR1
;102 = 12 VOLTS. 43= 5 VOLTS
;VOLTAGE REQUIRED-ADRES TO ERROR1
MOVLW 107 ;12V
MOVWF NO1_0
CLRF NO1_1
MOVF ADRES,W
MOVWF NO2_0
CLRF NO2_1
NEG16 NO2_0
MOVF NO1_0,W
ADDWF NO2_0,W
MOVWF ERROR1_0
SKPNC
INCF NO2_1,F
MOVF NO1_1,W
ADDWF NO2_1,W
MOVWF ERROR1_1
;****************************
;RESULT= ERROR1 * K1
CLRF NO1_1 ;CLEAR HIGH BYTE
MOVLW K1
MOVWF NO1_0
MOVF ERROR1_0,W
MOVWF NO2_0
MOVF ERROR1_1,W
MOVWF NO2_1
CALL SIGNEDMPY16
MOVF ANSER_0,W ;ANSER TO RESULT 16 BITS
MOVWF RESULT_0
MOVF ANSER_1,W
MOVWF RESULT_1
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;RESULT - (ERROR2 * K2)
CLRF NO1_1 ;CLEAR HIGH BYTE
MOVLW K2
MOVWF NO1_0
MOVF ERROR2_0,W
MOVWF NO2_0
MOVF ERROR2_1,W
MOVWF NO2_1
CALL SIGNEDMPY16
;SUBTRACT ANSER FROM RESULT
NEG16 ANSER_0 ;NEGATE 2ND WORD THEN ADD
MOVF ANSER_0,W
ADDWF RESULT_0,F
SKPNC
INCF RESULT_1,F
MOVF ANSER_1,W
ADDWF RESULT_1,F
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;RESULT + (ERROR3 * K3)
CLRF NO1_1 ;CLEAR HIGH BYTE
MOVLW K3
MOVWF NO1_0
MOVF ERROR3_0,W
MOVWF NO2_0
MOVF ERROR3_1,W
MOVWF NO2_1
CALL SIGNEDMPY16
;ADD ANSER TO RESULT
MOVF ANSER_0,W
ADDWF RESULT_0,F
SKPNC
INCF RESULT_1,F
MOVF ANSER_1,W
ADDWF RESULT_1,F
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
CALL SCALERESULT ;THIS DECIDES GAIN OF SYSTEM
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;SUBTRACT RESULT FROM PHASEANGLE TO GET CORRECT POSITION
BTFSC RESULT_1,7
GOTO NEGCHANGE
;ADD RESULT TO PHASEANGLE CHECKING FOR OVERFLOW
POSCHANGE
MOVF RESULT_0,W
ADDWF PHASEANGLE,W
BC PAOVERFLOWED
MOVWF PHASEANGLE
GOTO PHASEANGLEDONE
PAOVERFLOWED MOVLW 255
MOVWF PHASEANGLE
GOTO PHASEANGLEDONE
NEGCHANGE MOVF RESULT_0,W
ADDWF PHASEANGLE,W
BNC PAUNDERFLOWED
MOVWF PHASEANGLE
GOTO PHASEANGLEDONE
PAUNDERFLOWED
CLRF PHASEANGLE
PHASEANGLEDONE
;*********************************
;****************************
;PULSE MOSFET FOR PHASEANGLE
;****************************
MOVF PHASEANGLE,W
MOVWF TEMP
BZ RET ;NO MOSFET PULSE IF ZERO
BSF MOSFET ;MOSFET ON
WP1_ DECFSZ TEMP,F
GOTO WP1_
BCF MOSFET ;MOSFET OFF
RET
;;;;;;;;;;;;;;;;;;;;
GOTO MLOOP ;ENOUGH DELAY DOING MATHS TO DISCHARGE INDUCTOR.
END
;MACRO.ASM
;***********
;*************************
DEC16 MACRO ADDRESS
DECF ADDRESS,F
INCFSZ ADDRESS,W
INCF ADDRESS+1,F
DECF ADDRESS+1,F
ENDM
INC16 MACRO ADDRESS
INCFSZ ADDRESS,F
DECF ADDRESS+1,F
INCF ADDRESS+1,F
ENDM
NEG16 MACRO ADDRESS
COMF ADDRESS,F
COMF ADDRESS+1,F
INCFSZ ADDRESS,F
DECF ADDRESS+1,F
INCF ADDRESS+1,F
ENDM
NEG8 MACRO ADDRESS
COMF ADDRESS,F
INCF ADDRESS,F
ENDM
CLR16 MACRO ADDRESS
CLRF ADDRESS
CLRF ADDRESS+1
ENDM
RRSIGNED16 MACRO ADDRESS
RLF ADDRESS+1,W
RRF ADDRESS+1,F
RRF ADDRESS,F
ENDM
;************************************
;MATHS.ASM
;*********
MPY MACRO
LOCAL ZNOADD_40_0
CLRC ;ANSER * 2
RLF ANSER_0,F
RLF ANSER_1,F
;NO1 SHIFT LEFT ONE BIT INTO CARRY
RLF NO1_0,F ;16 BIT SHIFT
RLF NO1_1,F
BNC ZNOADD_40_0
;
;ADD NO2 TO ANSER
MOVF NO2_0,W
ADDWF ANSER_0,F
SKPNC ;WATCH FOR CARRY
INCF ANSER_1,F
MOVF NO2_1,W
ADDWF ANSER_1,F
;
ZNOADD_40_0
ENDM
;********************************
NEGNO1WORD NEG16 NO1_0
RETLW 0
;**********************************
NEGNO2WORD NEG16 NO2_0
RETLW 0
;**********************************
SIGNEDMPY16
;XOR B7'S TO FIND SIGN OF RESULT
MOVF NO1_1,W
XORWF NO2_1,W
MOVWF SIGN
;MAKE BOTH NUMBERS POSITIVE FOR UNSIGNED MPY
BTFSC NO1_1,7
CALL NEGNO1WORD
BTFSC NO2_1,7
CALL NEGNO2WORD
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
CLR16 ANSER_0
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
BTFSS SIGN,7 ;PUT BACK CORRECT SIGN
RETLW 0
NEG16 ANSER_0
RETLW 0
;**************************************
;INTH0.ASM
;*********
;*****************************************************
;SMPS PID PROGRAM FOR GENERAL USE. (C) N.P.WRIGHT 2010
;*****************************************************
INCLUDE MACRO.ASM
LIST R=DEC
LIST P=PIC12F510
;INCLUDE REGISTER DEFINITIONS
INCLUDE P12F510.INC
;==========================================================================
;
; Configuration Bits
;
;==========================================================================
;_IOSCFS_ON EQU H'0FFF' 8MHZ
;_IOSCFS_OFF EQU H'0FDF' 4MHZ
;_MCLRE_ON EQU H'0FFF'
;_MCLRE_OFF EQU H'0FEF'
;_CP_ON EQU H'0FF7'
;_CP_OFF EQU H'0FFF'
;_WDT_ON EQU H'0FFF'
;_WDT_OFF EQU H'0FFB'
;_LP_OSC EQU H'0FFC'
;_XT_OSC EQU H'0FFD'
;_IntRC_OSC EQU H'0FFE' INTERNAL RC OSC
;_ExtRC_OSC EQU H'0FFF'
__CONFIG _MCLRE_OFF & _WDT_OFF & _CP_OFF & _IntRC_OSC & _IOSCFS_ON
;*****************************************
;SUBWF FLAGS
;IF W > MEM THEN NC
;IF W = MEM THEN C
;IF W < MEM THEN C
;IF W<= MEM THEN C
;IF MEM>=W THEN C
;IF MEM<W THEN NC
;*****************************************
;******************************************
FIRSTRAM EQU 10 ;USED FOR DEBUG PURPOSES
CBLOCK FIRSTRAM
TEMP
NO1_0 ;NO1 (USED FOR MATHS)
NO1_1
NO2_0 ;NO2 (USED FOR MATHS)
NO2_1
ANSER_0 ;ANSWER (USED FOR MATHS)
ANSER_1
SIGN ;USED IN MATHS
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
ERROR1_0 ;NEWEST ERROR
ERROR1_1
ERROR2_0 ;PREVIOUS ERROR
ERROR2_1
ERROR3_0 ;PREVIOUS PREVIOUS ERROR
ERROR3_1
RESULT_0 ;USED IN MAIN CALCULATION
RESULT_1
PHASEANGLE ;CURRENT PHASEANGLE
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
ENDC
#DEFINE CARRYFLAG STATUS,0
;*******************************************
FIRSTA EQU 0 ;FIRST DATA TO PORTA
STATEA EQU 4
#DEFINE TESTPIN GPIO,4
#DEFINE MOSFET GPIO,1
#DEFINE A2D GPIO,2
;CONSTANTS USED FOR PID CONTROLLER (DONT CHANGE)
K1 EQU 49
K2 EQU 70
K3 EQU 25
;*********************************************
ORG 0
RESET GOTO AFTERRESET
;********************************
INCLUDE MATHS.ASM ;MATHS FUNCTIONS
;*************************************************
NEGRESULT16 NEG16 RESULT_0
RETLW 0
;**************************************
;GET A2D VALUE TO ADRES
READA2D BSF ADCON0,1 ;GO
NOP
NOP
READLP BTFSC ADCON0,1
GOTO READLP
RETLW 0
;**************************************
;THIS ALTERS THE GAIN OF THE SYSTEM
;HIGH GAIN CAUSES OVERSHOOT
;LOW GAIN CAUSES UNDERSHOOT
;
SCALERESULT
;THIS FIRST PART SCALES FOR BEST PERFORMANCE
;ALTHOUGH THIS MIGHT STILL CAUSE OVERFLOWS IN EXCEPTIONAL CASES
RRSIGNED16 RESULT_0 ;SCALING FACTOR = 2^N (GAIN)
RRSIGNED16 RESULT_0
RRSIGNED16 RESULT_0
RRSIGNED16 RESULT_0
RRSIGNED16 RESULT_0
; RRSIGNED16 RESULT_0
; RRSIGNED16 RESULT_0
; RRSIGNED16 RESULT_0
;THIS PART FORCES THE ANSWER TO BE IN THE RANGE -127 TO +127
;THIS PART ONLY COMES IN AT EXTREMES OF RANGES AND CONDITIONS
MOVF RESULT_1,W
MOVWF SIGN ;SAVE SIGN TO B7 OF SIGN
BTFSC RESULT_1,7 ;MAKE +VE TO SIMPLY ROUTINE
CALL NEGRESULT16
FIX MOVLW 80H ;CHECK IF IN RANGE YET. 0-127 IS IN RANGE
ANDWF RESULT_0,W
IORWF RESULT_1,W
BZ FIXED ;YES SO GO
CLRC ;+VE DIV BY 2
RRF RESULT_1,F
RRF RESULT_0,F
GOTO FIX ;RETEST
FIXED BTFSC SIGN,7 ;PUT SIGN BACK
CALL NEGRESULT16
RETLW 0
;*************************************
AFTERRESET
MOVWF OSCCAL
CLRWDT
MOVLW 55H
MOVWF CM1CON0 ;DISABLE COMPARATOR ON GPIO,1!
;SETUP PIC I/O AND OPTIONS
MOVLW FIRSTA
MOVWF GPIO
MOVLW STATEA
TRIS GPIO
MOVLW 0C0H ;WEAK PULL UPS OFF & WAKE UP ON PIN CHANGE OFF
OPTION
;SETUP A2D CONVERTER
MOVLW 69H ;CLOCK DIV 8
MOVWF ADCON0
;CLEAR MEMORY. DONT USE FSR AS TOP 2 BITS ARE SET
CLRF ERROR1_0
CLRF ERROR1_1
CLRF ERROR2_0
CLRF ERROR2_1
CLRF ERROR3_0
CLRF ERROR3_1
CLRF PHASEANGLE
;**************************************************************************************
;MAIN PROGRAM
;**************************************************************************************
MLOOP
BSF TESTPIN ;LISTON2
CALL READA2D
BCF TESTPIN ;LISTON2
;SHIFT DOWN ERRORS
MOVF ERROR2_0,W ;ERROR 2 TO ERROR 3
MOVWF ERROR3_0
MOVF ERROR2_1,W
MOVWF ERROR3_1
MOVF ERROR1_0,W ;ERROR 1 TO ERROR 2
MOVWF ERROR2_0
MOVF ERROR1_1,W
MOVWF ERROR2_1
;GET NEW ERROR1
;102 = 12 VOLTS. 43= 5 VOLTS
;VOLTAGE REQUIRED-ADRES TO ERROR1
MOVLW 107 ;12V
MOVWF NO1_0
CLRF NO1_1
MOVF ADRES,W
MOVWF NO2_0
CLRF NO2_1
NEG16 NO2_0
MOVF NO1_0,W
ADDWF NO2_0,W
MOVWF ERROR1_0
SKPNC
INCF NO2_1,F
MOVF NO1_1,W
ADDWF NO2_1,W
MOVWF ERROR1_1
;****************************
;RESULT= ERROR1 * K1
CLRF NO1_1 ;CLEAR HIGH BYTE
MOVLW K1
MOVWF NO1_0
MOVF ERROR1_0,W
MOVWF NO2_0
MOVF ERROR1_1,W
MOVWF NO2_1
CALL SIGNEDMPY16
MOVF ANSER_0,W ;ANSER TO RESULT 16 BITS
MOVWF RESULT_0
MOVF ANSER_1,W
MOVWF RESULT_1
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;RESULT - (ERROR2 * K2)
CLRF NO1_1 ;CLEAR HIGH BYTE
MOVLW K2
MOVWF NO1_0
MOVF ERROR2_0,W
MOVWF NO2_0
MOVF ERROR2_1,W
MOVWF NO2_1
CALL SIGNEDMPY16
;SUBTRACT ANSER FROM RESULT
NEG16 ANSER_0 ;NEGATE 2ND WORD THEN ADD
MOVF ANSER_0,W
ADDWF RESULT_0,F
SKPNC
INCF RESULT_1,F
MOVF ANSER_1,W
ADDWF RESULT_1,F
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;RESULT + (ERROR3 * K3)
CLRF NO1_1 ;CLEAR HIGH BYTE
MOVLW K3
MOVWF NO1_0
MOVF ERROR3_0,W
MOVWF NO2_0
MOVF ERROR3_1,W
MOVWF NO2_1
CALL SIGNEDMPY16
;ADD ANSER TO RESULT
MOVF ANSER_0,W
ADDWF RESULT_0,F
SKPNC
INCF RESULT_1,F
MOVF ANSER_1,W
ADDWF RESULT_1,F
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
CALL SCALERESULT ;THIS DECIDES GAIN OF SYSTEM
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;SUBTRACT RESULT FROM PHASEANGLE TO GET CORRECT POSITION
BTFSC RESULT_1,7
GOTO NEGCHANGE
;ADD RESULT TO PHASEANGLE CHECKING FOR OVERFLOW
POSCHANGE
MOVF RESULT_0,W
ADDWF PHASEANGLE,W
BC PAOVERFLOWED
MOVWF PHASEANGLE
GOTO PHASEANGLEDONE
PAOVERFLOWED MOVLW 255
MOVWF PHASEANGLE
GOTO PHASEANGLEDONE
NEGCHANGE MOVF RESULT_0,W
ADDWF PHASEANGLE,W
BNC PAUNDERFLOWED
MOVWF PHASEANGLE
GOTO PHASEANGLEDONE
PAUNDERFLOWED
CLRF PHASEANGLE
PHASEANGLEDONE
;*********************************
;****************************
;PULSE MOSFET FOR PHASEANGLE
;****************************
MOVF PHASEANGLE,W
MOVWF TEMP
BZ RET ;NO MOSFET PULSE IF ZERO
BSF MOSFET ;MOSFET ON
WP1_ DECFSZ TEMP,F
GOTO WP1_
BCF MOSFET ;MOSFET OFF
RET
;;;;;;;;;;;;;;;;;;;;
GOTO MLOOP ;ENOUGH DELAY DOING MATHS TO DISCHARGE INDUCTOR.
END
;MACRO.ASM
;***********
;*************************
DEC16 MACRO ADDRESS
DECF ADDRESS,F
INCFSZ ADDRESS,W
INCF ADDRESS+1,F
DECF ADDRESS+1,F
ENDM
INC16 MACRO ADDRESS
INCFSZ ADDRESS,F
DECF ADDRESS+1,F
INCF ADDRESS+1,F
ENDM
NEG16 MACRO ADDRESS
COMF ADDRESS,F
COMF ADDRESS+1,F
INCFSZ ADDRESS,F
DECF ADDRESS+1,F
INCF ADDRESS+1,F
ENDM
NEG8 MACRO ADDRESS
COMF ADDRESS,F
INCF ADDRESS,F
ENDM
CLR16 MACRO ADDRESS
CLRF ADDRESS
CLRF ADDRESS+1
ENDM
RRSIGNED16 MACRO ADDRESS
RLF ADDRESS+1,W
RRF ADDRESS+1,F
RRF ADDRESS,F
ENDM
;************************************
;MATHS.ASM
;*********
MPY MACRO
LOCAL ZNOADD_40_0
CLRC ;ANSER * 2
RLF ANSER_0,F
RLF ANSER_1,F
;NO1 SHIFT LEFT ONE BIT INTO CARRY
RLF NO1_0,F ;16 BIT SHIFT
RLF NO1_1,F
BNC ZNOADD_40_0
;
;ADD NO2 TO ANSER
MOVF NO2_0,W
ADDWF ANSER_0,F
SKPNC ;WATCH FOR CARRY
INCF ANSER_1,F
MOVF NO2_1,W
ADDWF ANSER_1,F
;
ZNOADD_40_0
ENDM
;********************************
NEGNO1WORD NEG16 NO1_0
RETLW 0
;**********************************
NEGNO2WORD NEG16 NO2_0
RETLW 0
;**********************************
SIGNEDMPY16
;XOR B7'S TO FIND SIGN OF RESULT
MOVF NO1_1,W
XORWF NO2_1,W
MOVWF SIGN
;MAKE BOTH NUMBERS POSITIVE FOR UNSIGNED MPY
BTFSC NO1_1,7
CALL NEGNO1WORD
BTFSC NO2_1,7
CALL NEGNO2WORD
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
CLR16 ANSER_0
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
MPY
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
BTFSS SIGN,7 ;PUT BACK CORRECT SIGN
RETLW 0
NEG16 ANSER_0
RETLW 0
;**************************************
Thank you nigelwright7557.
I may well have a go at making this work. A useful bit of code to have. I think the only PICs I've got with A/Ds are SMT, I don't want to make a PCB, so I'll have to get some in.
w
I may well have a go at making this work. A useful bit of code to have. I think the only PICs I've got with A/Ds are SMT, I don't want to make a PCB, so I'll have to get some in.
w
The 12f510 are less than a pound each in PDIP.
The only thing to watch out for is you need a small capacitor on the feedback into the PIC. The output has a bit of noise on it and this affects the PID a bit. A 100nf should be fine.
- Status
- Not open for further replies.