;############################################################################
;##               Canal Cam Turret Controller   V1.02.03                   ##
;##========================================================================##
;##  By Nathanael Nunes  8/24/2008                                         ##
;##------------------------------------------------------------------------##
;## Program accepts commands via serial to adjust angles of a turret with  ##
;## a camera mounted on it, report on current turret angles, and report    ##
;## temperture.                                                            ##
;## ** Project Goals **                                                    ##
;## 1Mhz operation for low power .....................................N/A  ##
;## 2 Pulse Servo controller .........................................100% ##
;## Error free Virtual UART ..........................................100% ##
;## ASCII command inturpreer .........................................100% ##
;## DS1620 Termometer communication .................................. 80% ##
;## Local display (LCD/7Seg Displays) ................................  0% ##
;############################################################################



device sx28l, turbo, stackx, OSC1MHZ 
IRC_CAL IRC_4MHZ
freq 1_000_000
reset main

;######################
;## Pin deffinitions ##
;######################

servox 	equ 	ra.0	;servo x data line
servoy 	equ 	ra.1	;servo y data line
tx_pin 	equ 	ra.2	;serial transmit line
rx_pin 	equ 	ra.3	;serial recive line
t1dq	equ	rb.0	;dq line for first 1620 therommeter
tclk	equ	rb.1	;clk line both 1620 thermometer
t1rst	equ	rb.2	;rst line for first 1620 thermometer
t2rst	equ	rb.3	;dq line for second 1620 thermometer

;########################################
;##         DS1620 command codes       ##
;########################################
Rconfig	equ	$ac	;Read Configuration
Wconfig	equ	$0c	;Write Configuration
CPU	equ	%10	;Serial Thermometer mode
NoCpu	equ	%00	;Stand alone thermostate mode
OneShot equ	%01	;one conversion per request
Cont	equ	$00	;Continuus conversions after start
StartC	equ	$ee	;Start Conversion
StopC	equ	$22	;Stop Conversion
Rtemp	equ	$aa	;Read Temperature

;################################
;##  Thermometer Variables     ##
;################################

org $50
DSdata	ds	1	;Byte for Comunication with DS1620
Dsbits	ds	1	;bit counter for coms with DS1620
DSflags	ds	1	;a flag holder for DS1620 stuff
DStemp1	ds	1	;counter for delays
DStemp2	ds	1	;counter for delays
DSsign	equ	DSflags.0	;Ninth bit for temperature values
DSclk9	equ	DSflags.1	;1 is 9bit transfer 0 is 8 bit transfer
fermath	ds	5	;verable space for ferenheight calculations


;################################################
;## Servo, Serial Com and misc Temp Variables  ##
;################################################

org $08
xcount	ds 	1	;how long x servo pulse has been on
ycount	ds 	1	;how long y servo pulse has been on
pulsepause	ds 	1	;how long both pulses are off
xngle	ds 	1	;desired angle value 0 to 10
yngle	ds 	1	;desired angle value 0 to 10
temp	ds	1
byte	ds	1
flags	ds	1 	;outbound charater


org $10
number_low	ds	1
number_high	ds	1
hex		ds	1
string		ds	1
tx_high		ds	1
tx_low		ds	1
tx_count	ds	1
tx_divide	ds	1
rx_count	ds	1
rx_divide	ds	1
rx_byte		ds	1
cdelay 		ds 	3
charin		ds 	1
mdex		ds	1
rx_flag		equ	flags.5

;#####################################
;##  Command inturpriter variables  ##
;#####################################

org $30

cmdl 		ds 	1
disptens	ds	2
bcd1s		ds	1
bcd10s		ds	1
bcd100s		ds	1
submath		ds	1


;###########################################################
;##  Irq Virtual Perifrials   Dual PWM  Drivers and UART  ##
;###########################################################
org $0
interrupt
	bank $10
;##########################################
;## PWM Driver Part I.   Pulse time test ##
;##########################################
	jnb servox, SYtestoff	;If servox pulse ins't on go check y
	djnz xcount, SYtest	;should servo x pulse stay on?
	clrb servox 		;no turn it off.
SYtest				;y servo test assumes that x pulse was on
	jnb servoy, idone	;if y pulse isn't on we are done
	djnz ycount, idone	;should y pulse stay on
	clrb servoy		;no turn it off
	jmp idone		;we are done with this cycle.   
SYtestoff			;test y pulse state asuminh x was off
	jnb servoy, ptime	;if y pulse is off go to pause section
	djnz ycount, idone	;if y should stay on end this cycle
	clrb servoy		;if not turn the y pulse off

;########################################
;## PWM Driver Part II.  Pause Timmer  ##
;## 20ms between pulses.               ##
;########################################

ptime
	csne pulsepause, #0	;is this the first time we arved here
	jmp pinitalize		;yes go setup the pulse
	djnz pulsepause,idone 	;is the pause over, end if not
	mov w, xngle		;load x pulse time to timmer
	mov xcount, w
	add xcount, #10		;add 1ms to pulse
	mov w, yngle		;load y pulse time to timmer
	mov ycount, w
	add ycount, #10		;add 1ms to pulse
	setb servox		;turn on x pulse
	setb servoy		;turn on y pulse
	jmp idone  
pinitalize
	mov pulsepause, #199	;set pause time to 20ms 
idone	;begin uart virtual parifrial

;################################################################
;##                 Virtual Peripheral Uart                    ##
;##============================================================##
;##  Based on code writen by Chip Gracey and Craig Webb        ##
;## modified to run at 2400 Baud only with a 1mhz clock.  Irq  ##
;## runs once every 104uS.  Each byte is only 4 Irq cycles     ##
;## long.                                                      ##
;################################################################               

:transmit
	clrb tx_divide.2
	inc tx_divide
	stz
	snb tx_divide.2
	test tx_count
	jz :receive
	clc
	rr tx_high
	rr tx_low
	dec tx_count
	movb tx_pin, /tx_low.6
:receive
	movb c, rx_pin
	test rx_count
	jnz :rxbit
	mov w, #9
	sc
	mov rx_count, w
	mov rx_divide, #7	;find center of pulse 6, 7 toggle the center
:rxbit
	djnz rx_divide, end_int
	setb rx_divide.2
	dec rx_count
	sz
	rr rx_byte
	snz
	setb rx_flag
end_int
	mov w, #-104	;  .104ms
	retiw
get_byte
	bank $10
	jnb rx_flag, $
	clrb rx_flag
	mov byte, rx_byte
send_byte
wait
	test tx_count
	jnz wait

	not w
	mov tx_high, w
;###################################
;##  Stop bit compatability pause ##
;##===============================##
;##  Delay inforces longer stop   ##
;## bit when sending strings, at  ##
;## this slow speed stob and start##
;## bit timing can cause write    ##
;## errors based on cpu           ##
;## fluctiations.                 ##
;###################################

	mov cdelay, #60
qpdl
	decsz cdelay
	jmp qpdl
	setb tx_low.7
	mov tx_count, #10
	retp


tdelay
	mov cdelay, #1
tdelay2
	decsz cdelay
	jmp tdelay2
dell2
	decsz cdelay+1
	jmp tdelay
	decsz cdelay+2
	jmp tdelay
;call get_byte
	ret
;##########################################
;##  Message reader.   Jump to table at  ##
;## position W and return the character  ##
;## stored there.                        ##
;##########################################
m_read
	page menutable
	jmp w

org $100
main
;#########################
;##   Setup pins in SX  ##
;#########################
	mov !ra, #%1000	;set pins a to output
	mov ra,	#%0100	;no pulse to force intal setup.
	mov rb, #%00000000	;clear DS1620 com lines
	mov !rb, #$00000000	;set DS1620 list to output mode
;###########################
;##     Clear all ram     ##
;##=======================##
;##  Code from programing ##
;## the SX Mirocontroler  ##
;## A Complete Guide      ##
;## By Gunther Daubach    ##
;###########################


	clr fsr
ClearData
	sb fsr.4
	setb fsr.3
	clr ind
	ijnz fsr, ClearData
;############################################
;##  Turn on Inturupt and set to initilize ##
;## first servo pulse when it runs         ##
;############################################

mov !OPTION, #%10011111 ; Enable RTCC rollover interrupt
			; RTCC inc on clock, no prescale

mov pulsepause, #1	;force inital pulse setup

;##############################################
;##  Bootup:    Test servo movment and setup ##
;## DS1620s so we can read values from them. ##
;##############################################

bank $10
mov xngle, #0
mov yngle, #0
;************ DEBUG BLOCK *************
;bank $30
;mov disptens+1, #11
;mov cmdl, #4
;mov byte, #13
;jmp cmdtest_j ;   debug stuff
;jmp dsinitize 
;************ DEBUG BLOCK *************
call tdelay
mov xngle, #11
call tdelay
mov xngle, #5
call tdelay
mov yngle, #11
call tdelay
mov yngle, #5
;<< Add code to initilize DS1620s >>
dsinitize
;ds1
bank $50
setb tclk
mov DSdata, #Wconfig
call @DSsout1
mov DSdata, #CPU
or DSdata, #Cont
call @DSsout1
clrb t1rst
bank $10
call tdelay
bank $50
mov DSdata, #StartC
call @DSsout1
clrb t1rst
bank $10
call tdelay

;ds2
bank $50
setb tclk
mov DSdata, #Wconfig
call @DSsout2
mov DSdata, #CPU
or DSdata, #Cont
call @DSsout2
clrb t2rst
bank $10
call tdelay
bank $50
mov DSdata, #StartC
call @DSsout2
clrb t2rst
bank $10
call tdelay

;end initilaztion

mov w, #msgready	; Set mesage index for displaying READY>
jmp dispmm		;display message and begin main loop


;###############################
;##  Program loop begins here ##
;###############################
state1
bank $10
	call get_byte	;Pickup the serial character recived
	cse byte, #"?" 	;is it the help button?
	jmp cmdtest_j	;No jump to comand inturpreter rutine
	clr w		;Yes Prepare to Send Help Message

;###############################################
;##                   dispmm                  ##
;##===========================================##
;##  Displayes pre defigned strings using     ##
;## message starting at position W.           ##
;###############################################

dispmm
	mov mdex, w	;store of first character in message 
dispmmr
	mov w, mdex	;what message character to look at
	call m_read	;get character
	page main
	test w		;is it a zerro (end of message)
	snz		
	jmp state1	;yes were done
	call send_byte	;no send character to serial port
	inc mdex	;prepare to read next character
	jmp dispmmr	;Start prossess again
cmdtest_j
page cmdtest
jmp cmdtest
;##############################################
;##                  dis_n                   ##
;##==========================================##
;##  Output 2 digit number via serial.       ##
;## Number is already in BCD with the tens   ##
;## digit stored in disptens, and the ones   ##
;## digit stored in disptens+1.              ##
;##############################################
dis_n
	bank $30
	mov w, disptens		;load tens number
	bank $10
	call send_byte		;display tens number
	bank $30
	mov w, disptens+1	;load ones number
	clr disptens		;reset disptens variables
	clr disptens+1
	bank $10
	call send_byte		;send ones number
	jmp state1

;###########################################
;##    SECTION TWO COMMAND INTERPRETER    ##
;##=======================================##
;##  This section contains subrutines and ##
;## main rutines for inturpreting, and    ##
;## responding to commands recived via    ##
;## serial.                               ##
;###########################################

org $400
cmdtest
	bank $30
	csne byte, #"!"
	clr cmdl 	;if its the command header reset and restart
	mov w, cmdl
	jmp pc+w	;what level of the command structure are we in
	jmp cinit	;no command level yet
	jmp r_or_w	;waiting on read or write
	jmp w_servo	;writing but don'tknow what servo
	jmp r_servo	;reading but don't know what servo
	jmp wx_servo	;writing x servo wait on digit 1
	jmp wy_servo	;writing y servo wiat on digit 1
	jmp rx_servo	;just waiting for enter to send
	jmp ry_servo	;just waiting for enter to send
	jmp r_temp	;read temperature mode
bin2bcd	;8 bit binary to bcd converter
		;w is broken into bcd and stored in
		;in bcd1s, bcd10s, and bcd100s.
	clr bcd10s
	clr bcd100s
	mov bcd1s, w	;reset variables and load w to 1s columb
bin2bcd_1c
	csa, bcd1s, #9	;is there a tens columb
	jmp bin2bcd_10c	;test the tens columb
	inc bcd10s	;we had some tens so lets add on	
	stc
	sub bcd1s, #10	;take ten from 1s columb
	jmp bin2bcd_1c	;repet prosess
bin2bcd_10c
	csa bcd10s, #9	;is there a hundreds columb
	jmp bin2bcd_exit
	inc bcd100s
	stc
	sub bcd10s, #10
	jmp bin2bcd_10c
bin2bcd_exit
	retp

;#########################################################
;##  Twos Componnent Negative Number to Absolute Value  ##
;##-----------------------------------------------------##
;##  Asumming we had a 9 bit twos componnent number     ##
;## take the lower 8 bits (stored in W) and return the  ##
;## absolut value in as a standard 8 bit number.        ##
;#########################################################

twos2abs		;get absolute value from 8 bit negative number
	mov submath, w	;place the value in a temperary variable
	dec submath	;subtract to convert to onse component
	not submath	;convert to absolute number
	mov w, submath	;put new value back in w
	ret


cinit
	csne byte, #"!"		;is this the command header?
	skip			;yes proceed
	jmp state1_r		;no return to the main loope
	inc cmdl		;changle command level to 1
	jmp state1_r		;go back to main loop
r_or_w
	cje byte, #"R", make_r	;was the last char a r or w
	cje byte, #"r", make_r	;if so prepare for next step
	cje byte, #"W", make_w	
	cje byte, #"w", make_w
	jmp cmderrout		;didn't recive a valade verriable so
				;reset variables and assume we have
				;nothing.

;	mov w, #mesg1		;if not r or w we have an error
;	clr cmdl		;not a valad command reset and restart
;	bank $10
;	jmp dispmm_r
make_r
	mov w, #3
	skip
make_w	mov w, #2
	mov cmdl, w
	jmp state1_r 
w_servo
	cje byte, #"x", make_wx	;what servo to write to?
	cje byte, #"X", make_wx
	cje byte, #"y", make_wy
	cje byte, #"Y", make_wy
	jmp cmderrout	;no valad argument recived reset and restart

;	mov w, #mesg1		;debug message to show arivlal
;	clr cmdl		;not a valad command reset and restart
;	bank $10
;	jmp dispmm_r
make_wy
	mov w, #5
	skip
make_wx
	mov w, #4
	mov cmdl, w
	jmp state1_r
r_servo
	cje byte, #"x", make_rx	;what servo to read value from?
	cje byte, #"X", make_rx
	cje byte, #"y", make_ry
	cje byte, #"Y", make_ry
	cje byte, #"t", make_rt ;temperature read
	cje byte, #"T", make_rt
	jmp cmderrout		;no valad agument restet and restart
;	mov w, #mesg1		;debug message to show arivlal
;	clr cmdl		;not a valad command reset and restart
;	bank $10
;	jmp dispmm_r
make_ry
	mov w, #7		;prepare to read servo y if next char is enter
	skip
make_rx
	mov w, #6		;prepare to read servo x if next char is enter
	mov cmdl, w
	jmp state1_r
make_rt
	mov w, #8
	mov cmdl, w
	jmp state1_r
wx_servo
	csne byte, #13		;was enter pressed
	jmp submit_x		;yes then submit value
	mov disptens, byte	;prepare to accept a number
	stc
	sub disptens, #48	;convert ascii to binary
	csb disptens, #10	;is it realy a number
	jmp cmderrout		;bad inpute kill command
	test disptens+1		;have we stored a value already?
	snz
	jmp addtovall		;no  store this one
	mov w, disptens+1
	clc
	add disptens+1 , w	;multiply by 5
	add disptens+1 , w
	add disptens+1 , w
	add disptens+1 , w
;	add disptens+1 , w	;only need 4 because disptens already has one
	rl disptens+1		;multiply by 2   (now we have times 10)

addtovall
	clc
	add disptens+1, disptens	;add new number to old
	jmp state1_r

submit_x
	csa disptens+1, #12	;too high will fry servo
	jmp x_submit
	mov disptens+1, #13	;number was to high so substituting 13
x_submit
	mov xngle, disptens+1	;store the value residing dispens as the x angle
	clr disptens
	jmp bintobcn		;echo back the new vaolue as confermation
wy_servo
	csne byte, #13		;was enter pressed
	jmp submit_y		;yes then submit value
	mov disptens, byte	;prepare to accept a number
	stc
	sub disptens, #48	;convert ascii to binary
	csb disptens, #10	;is it realy a number
	jmp cmderrout		;bad inpute kill command
	test disptens+1		;have we stored a value already?
	snz
	jmp addtovally		;no  store this one
	mov w, disptens+1
	clc
	add disptens+1 , w	;multiply by 5
	add disptens+1 , w
	add disptens+1 , w
	add disptens+1 , w
;	add disptens+1 , w	;only need 4 because disptens already has one
	rl disptens+1		;multiply by 2   (now we have times 10)

addtovally
	clc
	add disptens+1, disptens	;add new number to old
	jmp state1_r
submit_y
	csa disptens+1, #12	;too high will fry servo
	jmp y_submit
	mov disptens+1, #13	;number was to high so substituting 13
y_submit
	mov yngle, disptens+1	;store the value residing dispens as the x angle
	clr disptens
	jmp bintobcn		;echo back the new vaolue as confermation
	jmp state1_r
rx_servo
	csne byte, #13		;was enter pressed
	jmp display_x		;yes exicute command
	mov w, #mesg1		;debug message to show arivlal
	clr cmdl		;not a valad command reset and restart
	bank $10
	jmp dispmm_r

ry_servo
	csne byte, #13		;was enter pressed
	jmp display_y		;exicute command
	mov w, #mesg1		;debug message to show arivlal
	clr cmdl		;not a valad command reset and restart
	bank $10
	jmp dispmm_r
display_x
	clr disptens
	mov disptens+1, xngle	;load x servo angle for ascii convesion
bintobcn			;conver binary to bcd less than 100
	mov w, disptens+1
	call bin2bcd	;get bcd values from w
	mov disptens+1, bcd1s
	mov disptens, bcd10s
;	csa disptens+1, #9	;is the number less than ten
;	jmp tcondone		;yes conversion doen
;	inc disptens		;no add one to the tens columb
;	stc
;	sub disptens+1, #10	;take 10 from the binary number
;	jmp bintobcn		;test again
tcondone			;convert bcd to ascii
	clc
	add disptens, #48	;convert number to ascii
	add disptens+1, #48	;convert number to ascii
	clr cmdl
	page main
	jmp dis_n
display_y
	clr disptens
	mov disptens+1, yngle	;load y servo for ascii conversion
	jmp bintobcn
r_temp			;read temperature command
	csne byte, #13
	jmp r_temp_exicute
	csne byte, #"i"
	jmp r_temp_i
	csne byte, #"I"
	jmp r_temp_i
	csne byte, #"o"
	jmp r_temp_o
	csne byte, #"O"
	jmp r_temp_o
	jmp cmderrout 
r_temp_o
	mov disptens, #1
	jmp state1_r
r_temp_i
	mov disptens+1, #1
	jmp state1_r
r_temp_exicute
;fist stab assume we pick temperature probe 1 and just let it go.
	csne disptens, #1
	jmp disp_o_temp
	csne disptens+1, #1
	jmp disp_i_temp
	jmp cmderrout

disp_i_temp		;read indoor ds1620
	bank $50
	mov DSdata, #Rtemp
	call @DSsout1
	call @DSsin1
	jmp tempout
disp_o_temp		;read outdoor ds1620
	bank $50
	mov DSdata, #Rtemp
	call @DSsout2
	call @DSsin2

tempout
	mov w, DSdata
	jnb DSsign, Conttempout
	bank $30
	call @twos2abs
	bank $50
Conttempout
	
	call @raw2fer
	;clc
	;rr DSdata	;devide raw number by 2 to get celsious
	;mov w, DSdata
	bank $30
	call bin2bcd
	clc
	add bcd100s, #48
	clc
	add bcd10s, #48
	clc
	add bcd1s, #48
	mov w, bcd100s
	bank $50
	snb DSsign
	mov w, #"-"
	bank $10
	call @send_byte
	bank $30
	mov w, bcd10s
	bank $10
	call @send_byte
	bank $30
	mov w, bcd1s
	bank $10
	call @send_byte	
	bank $30
	clr cmdl
	clr disptens
	clr disptens+1

	jmp state1_r



state1_r
	page main
	jmp state1
dispmm_r
	page main
	jmp dispmm
cmderrout
	clr disptens
	clr disptens+1
	mov w, #mesg1		;debug message to show arivlal
	clr cmdl		;not a valad command reset and restart
	bank $10
	jmp dispmm_r	
	
org $600
DSsout1
	bank $50
	mov !rb, #0
	mov Dsbits, #9
	movb c, DSsign
:begin
	setb t1rst
:loop
	rr DSdata
	clrb tclk
	movb t1dq, c
	mov w, --Dsbits
	snz
	snb DSclk9
	setb tclk
	djnz Dsbits, :loop
	retp
	
DSsin1
	bank $50
	mov !rb, #1
	mov Dsbits, #9
	clc
:loop
	rr DSdata
	clrb tclk
	movb c, t1dq
	setb tclk
	djnz Dsbits, :loop
	clrb t1rst
	retp 

DSsout2
	bank $50
	mov !rb, #0
	mov Dsbits, #9
	movb c, DSsign
:begin
	setb t2rst
:loop
	rr DSdata
	clrb tclk
	movb t1dq, c
	mov w, --Dsbits
	snz
	snb DSclk9
	setb tclk
	djnz Dsbits, :loop
	retp

DSsin2
	bank $50
	mov !rb, #1
	mov Dsbits, #9
	clc
:loop
	rr DSdata
	clrb tclk
	movb c, t1dq
	setb tclk
	djnz Dsbits, :loop
	clrb t2rst
	retp 

raw2fer		;convert raw ds1620 data to ferenhight value
	bank $50
	mov fermath, w
	mov fermath+1, w
	mov fermath+2, w
	clr fermath+3
:fermathloop
	csa fermath+2, #9
	jmp :out
	inc fermath+3
	stc
	sub fermath+2, #10
	jmp :fermathloop
:out
	clc
	rr fermath+1
	stc
	sub fermath+1, fermath+3	; 8 tenths celsious temp
	clc
	rr fermath	;raw value in celsious
	clc
	add fermath, fermath+1		;celsios temmp  * 1.8
	snb DSsign	;is it less than 0 celsious
	jmp negative

	clc		;no carry on as normal
	add fermath, #32	;add 32 to cels temp * 1.8
	mov w, fermath	
	retp
negative
	cja fermath, #32, stillneg	;will it be less than 0 ferenhight
	mov fermath+4, #32		;no invert math to keep posative number
	stc
	sub fermath+4, fermath
	clrb DSsign
	mov w, fermath+4
	retp 
stillneg
	stc
	sub fermath, #32		;increase neg number by 32
	mov w, fermath		
	retp
org $200
menutable
retw 13, 10, 13, 10, "Natpie Cammera Turret Firmware V 1.02" , 13, 10
retw "! <-- Command start", 13, 10
retw "W or R <---- write or read vallue", 13, 10
retw "X or Y <---- Servo value to read or write", 13, 10
retw "2 digit number (write only) 0 to 11 for server angle", 13, 10
retw "Enter <--- exicute command", 13, 10
msgready
retw "READY>", 0
mesg1
retw 13,10, "Command Error", 0
