;
;	Software regulated bench power supply
;
;	Full PID implementation of voltage regulation
;
	
	device	PIC16F870,pwrt_off,wdt_on,hs_osc

SRDAT	=	rc.1
SRCLK	=	rc.3
LEDON	=	rc.7


	org	20h
	
butime	ds	2
func	ds	1
vsave	ds	1
data	ds	1
temp	ds	1
temp1	ds	1
count	ds	1
pwidth	ds	1
lpcnt	ds	1
lpxnt	ds	1
offcnt	ds	1
slowf	ds	1
hi	ds	1
low	ds	1
zhi	ds	1
zlow	ds	1
thi	ds	1
tlow	ds	1
adjhi	ds	1
adjlo	ds	1
vhi	ds	1
vlo	ds	1
lasthi	ds	1
lastlo	ds	1
alo	ds	1
ahi	ds	1
amax	ds	1
pwmhi	ds	1
pwmlo	ds	1
vset	ds	1
xvset	ds	1
shaft	ds	1
del0	ds	1
del1	ds	1
clean	ds	1
seqnr	ds	1
vsegs	ds	3
asegs	ds	3

flag	ds	1

NZ	=	flag.0
OKBUT	=	flag.1		;ok for next button
BHIT	=	flag.2		;button is hit
DOWDT	=	flag.3		;enable watchdog in delay loop
BUTON	=	flag.4		;button state
BHELD	=	flag.5		;button held down

flag1	ds	1

RINC	=	flag1.0		;rotor increment
RDEC	=	flag1.1		;rotor decrement

	org	0
	
	goto	start
	
	org	4
	reti
	
drvseg	addwf	PCL
	retw	90h,0A0h,0B0h,0D0h,0E0h,0F0h

digseg	addwf	PCL
	retw	3fh,6h,5bh,4fh,66h,6dh,7dh,7h,7fh,67h

funcs	addwf	PCL
	retw	77h,38h,6	;ALI


start	clrf	status
	movlw	0Ch		;set pwm mode
	movwf	CCP1CON
	bsf	status,5
	movlw	11111b		;set for analog inputs
	movwf	TRISA
	movlw	7
	movwf	TRISB
	movlw	0
	movwf	TRISC
	movlw	255
	movwf	PR2		;set pwm period
	movlw	80h		;right justify 5 analog input channels
	movwf	ADCON1
	bcf	status,5
	movlw	4
	movwf	T2CON
	clrf	pwmlo
	clrf	pwmhi
	clrf	lasthi
	clrf	lastlo
	clrf	alo
	clrf	ahi
	clrf	seqnr
	clrf	data
	clrf	vset
	movlw	200
	movwf	amax
	movf	rb,w
	movwf	shaft


cycle	clrwdt
	call	getvlt		;read the voltage
	call	efac2		;get the proportional error
	call	clamp		;limit amount
	call	newpwm		;+/- to power
	call	diffi		;get the differencial error
	call	newpwm		;adjust power
	call	chekov		;check limits
	call	slowp		;do the integral
	call	setpwm		;send to the hardware
	decfsz	lpcnt
	goto	cycle
	movlw	20
	movwf	lpcnt
	call	readsi		;read the input
	call	nxtdsp
	call	dispv	
	call	calcv
	call	onoff
	call	getamp		;read amps
	decfsz	lpxnt
	goto	cycle
	movlw	200
	movwf	lpxnt
	call	dispa		;display amps
	goto	cycle


onoff	btfsc	BUTON
	goto	:byp
	movf	alo,w
	movwf	low
	movf	ahi,w
	movwf	hi
	rrf	hi
	rrf	low
	movf	amax,w
	subwf	low,w
	btfsc	c
	goto	:off		;power off.
:byp	call	button
	btfss	BHIT
	ret
	bcf	BHIT
:off	clrf	hi
	clrf	low
	call	dopwm		;power off
:wait	clrwdt
	call	button
	btfss	BHIT
	goto	:cont
	bcf	BHIT
	movf	rb,w
	movwf	shaft
	ret
:cont	movlw	50
	call	delay1
	clrf	vsegs
	clrf	vsegs+1
	clrf	vsegs+2
	clrf	asegs
	clrf	asegs+1
	clrf	asegs+2
	bsf	asegs+2,7
	call	nxtdsp
	call	chkset
	goto	:wait


button	btfss	rb,0		;read switch
	goto	:but		;button down - toggle state
	bcf	BUTON
	bcf	BHELD
	bcf	BHIT
	movlw	2
	movwf	butime+1
	clrf	butime
	decfsz	offcnt
	ret
	bsf	OKBUT
	ret
:but	movlw	50
	movwf	offcnt
	decfsz	butime
	goto	:ok
	decfsz	butime+1
	goto	:ok
	bsf	BHELD
:ok	bsf	BUTON
	btfss	OKBUT
	ret
	bcf	OKBUT
	bsf	BHIT		;button hit valid
	ret


chkset	btfss	BHELD		;check button state
	ret
	bcf	BHELD
	clrf	func
:wait	clrwdt
	call	button
	btfsc	BHELD
	goto	:back
	call	showf
	call	setmax
	movf	amax,w
	movwf	low
	clrf	hi
	bcf	c
	rlf	low
	rlf	hi
	movlw	asegs
	call	dexi
	bsf	asegs,7
	call	nxtdsp
	bcf	BHIT
	movlw	50
	call	delay1
	goto	:wait
:back	bcf	BHELD
	ret

showf	movf	func,w
	addwf	func,w
	addwf	func,w		;func x 3
	movwf	temp
	call	funcs
	movwf	vsegs
	incf	temp
	movf	temp,w
	call	funcs
	movwf	vsegs+1
	incf	temp
	movf	temp,w
	call	funcs
	movwf	vsegs+2
	ret

; read shaft encoder	

readsi	call	rotor
	btfsc	RINC
	goto	:more
	btfsc	RDEC
	goto	:less
	ret
:less	movf	vset		;set z flag
	btfsc	z
	goto	:done
	decf	vset
	goto	:done
:more	incf	vset
	movf	vset,w
	xorlw	201
	btfss	z
	goto	:done
	decf	vset
:done	ret

setmax	call	rotor
	btfsc	RINC
	goto	:more
	btfsc	RDEC
	goto	:less
	ret
:less	movf	amax
	btfsc	z
	ret
	decf	amax
	ret
:more	incf	amax
	movf	amax,w
	xorlw	251
	btfss	z
	ret
	decf	amax
	ret

	

rotor	bcf	RINC
	bcf	RDEC
	movf	rb,w		;get quad bits
	movwf	temp
	movwf	data
	xorwf	shaft,w
	andlw	6		;isolate bits
	btfsc	z		;match?
	ret			;yes - return z-set
	movf	shaft,w
	rlf	temp
	xorwf	temp
	btfsc	temp,2
	goto	:up
:down	decf	xvset
	goto	:ok
:up	incf	xvset
:ok	btfss	data,1
	goto	:skip
	btfss	data,2		;both bits low ?
	goto	:skip		;no.
	movf	xvset,w
	xorlw	4
	btfsc	z
	goto	:more
	movf	xvset,w
	xorlw	-4
	btfsc	z
	goto	:less
	goto	:done
:less	bsf	RDEC
	goto	:done
:more	bsf	RINC
:done	clrf	xvset
:skip	movf	data,w
	movwf	shaft
	ret

	

calcv	movf	vset,w
	movwf	low
	movlw	73
	call	mult
	rrf	hi
	rrf	low
	rrf	hi
	rrf	low
	rrf	hi
	rrf	low
	rrf	hi
	rrf	low
	movf	low,w
	movwf	adjlo
	movf	hi,w
	andlw	3
	movwf	adjhi
	ret


slowp	decfsz	slowf		;get timeing factor
	ret
	movlw	20
	movwf	slowf
	movf	vhi,w
	subwf	adjhi,w
	btfsc	z
	goto	:chklo
:chk	btfss	c
	goto	:dn
	incf	pwmlo
	btfsc	z
	incf	pwmhi
	ret
:dn	decf	pwmlo
	movf	pwmlo,w
	xorlw	0ffh
	btfsc	z
	decf	pwmhi
	movf	pwmlo,w
	xorlw	0ffh
	btfss	z
	ret
	clrf	pwmlo
	clrf	pwmhi
	ret
:chklo	movf	vlo,w
	subwf	adjlo,w
	btfss	z		;exact match?
	goto	:chk
	ret			;no - action.


diffi	clrf	hi
	movf	vlo,w
	subwf	lastlo,w	;find last reading
	movwf	low
	btfss	c
	decf	hi
	movf	vhi,w
	subwf	lasthi,w
	addwf	hi
	movf	vlo,w
	movwf	lastlo
	movf	vhi,w
	movwf	lasthi
	ret
	

; get error factor in hi/low

efac1	clrf	hi
	movf	vlo,w
	subwf	adjlo,w
	movwf	low
	btfss	c
	decf	hi
	movf	vhi,w
	subwf	adjhi,w
	addwf	hi
	ret

efac2	call	efac1
	btfsc	hi,7		;negative value?
	goto	:neg
	bcf	c
	rrf	hi
	rrf	low
	bcf	c
	rrf	hi
	rrf	low
	ret
:neg	rrf	hi
	rrf	low
	bsf	hi,7
	rrf	hi
	rrf	low
	bsf	hi,7
	ret
	

clamp	btfsc	hi,7
	ret			;only process positive corrections
	movf	hi,w
	btfss	z
	goto	:max
	movf	low,w
	sublw	10
	btfss	c
	goto	:max
	ret
:max	movlw	10
	movwf	low
	clrf	hi
	ret
	
; adjust new PWM value using hi/low

newpwm	movf	low,w
	addwf	pwmlo
	btfsc	c
	incf	pwmhi
	movf	hi,w
	addwf	pwmhi
	ret
	
; check PWM value for in range 0 to 3ff.

chekov	btfsc	pwmhi,7		;underflow?
	goto	:min
	movf	pwmhi,w
	andlw	0FCh		;overflow?
	btfsc	z
	ret			;no - in range
	movlw	3
	movwf	pwmhi
	movlw	0FFh
	movwf	pwmlo
	ret
:min	clrf	pwmhi
	clrf	pwmlo
	ret


; sets 10 bit pwm value in CCP1CON<5,4>/CCPR1L<7-0>

setpwm	movf	pwmlo,w
	movwf	low
	movf	pwmhi,w
	movwf	hi
dopwm	movf	low,w
	movwf	temp
	swapf	temp
	movf	temp,w
	andlw	30h
	iorlw	0Ch		;set for pwm mode
	movwf	CCP1CON
	rrf	hi
	rrf	low
	rrf	hi
	rrf	low		;hi 8 bits
	movf	low,w
	movwf	CCPR1L
	ret



; multiply w by low into low/hi

mult	movwf	temp
	movlw	8
	movwf	count
	movf	low,w
	movwf	temp1
	clrf	low
	clrf	hi
	movf	temp,w
	bcf	c
:loop	rrf	temp1
	btfsc	c
	addwf	hi
	rrf	hi
	rrf	low
	decfsz	count
	goto	:loop
	ret



getvlt	movlw	0
	call	atod
	movf	low,w
	movwf	vlo
	movf	hi,w
	movwf	vhi
	ret

getamp	movlw	1
	call	atod
	movf	low,w
	movwf	alo
	movf	hi,w
	movwf	ahi
	ret


	

dispv	movf	vset,w
	movwf	low
	clrf	hi
	movlw	vsegs
	call	dexi
	bsf	vsegs+1,7
	ret
	
	
dispa	movf	alo,w
	movwf	low
	movf	ahi,w
	movwf	hi
	movlw	asegs
	call	dexi
	bsf	asegs,7
	ret
	
	
; decimal values up to 1000 converted directly into
; segment values

dexi	movwf	FSR		;place to put data
	bcf	NZ
	movlw	100<
	movwf	thi
	movlw	100>
	movwf	tlow
	call	:digit
	movlw	10<
	movwf	thi
	movlw	10>
	movwf	tlow
	call	:digit
	movlw	1<
	movwf	thi
	movlw	1>
	movwf	tlow
	call	:digit
	ret

:digit	clrf	count
:dig1	call	dsub
	btfsc	hi,7
	goto	:dig2
	bsf	NZ
	incf	count
	goto	:dig1
:dig2	call	dadd
	movf	count,w
	sublw	9
	btfss	c
	goto	:err
	movf	count,w
	call	digseg		;get segment data
:done	movwf	0		;store.
	incf	FSR
	ret
:err	movlw	49h
	goto	:done
	
dsub	movf	tlow,w
	subwf	low
	btfss	c
	decf	hi
	movf	thi,w
	subwf	hi
	ret

dadd	movf	tlow,w
	addwf	low
	btfsc	c
	incf	hi
	movf	thi,w
	addwf	hi
	ret

nxtdsp	incf	seqnr
	movf	seqnr,w
	xorlw	6		;past end of list?
	btfss	z
	goto	:ok
	clrf	seqnr
:ok	bcf	LEDON		;disable all displays
	movf	seqnr,w
	addlw	vsegs		;base of segment data
	movwf	FSR
	movf	0,w		;get segment data
	movwf	temp
	movlw	8
	movwf	count
:shft	bcf	SRDAT
	btfss	temp,7		;hi order first
	bsf	SRDAT		;reverse sense
	bsf	SRCLK
	bcf	SRCLK		;shift it out
	rlf	temp
	decfsz	count
	goto	:shft
	movf	seqnr,w
	call	drvseg
	movwf	temp
	movf	rc,w
	andlw	0Fh
	iorwf	temp,w
	movwf	rc		;select new digit
	ret

	
; read atod channel set by 'w'
; result in hi/low

atod	movwf	temp		;get channel (0-4)
	bcf	c
	rlf	temp
	rlf	temp
	rlf	temp		;put in place
	movlw	81h		;/32 clock, power up
	iorwf	temp,w
	movwf	ADCON0
	movlw	2
	call	delay
	bsf	ADCON0,2
	nop
	nop
:chk	btfsc	ADCON0,2	;check for conversion complete
	goto	:chk
	bsf	status,5
	movf	ADRESL,w
	bcf	status,5
	movwf	low
	movf	ADRESH,w
	movwf	hi
	ret
	

delay1	bsf	DOWDT
delay	movwf	del1
:d1	movwf	del0
:d2	btfsc	DOWDT
	clrwdt
	decfsz	del0
	goto	:d2
	decfsz	del1
	goto	:d1
	bcf	DOWDT
	ret
	
	end
	





