;*****************************Servo test*********************************
;.include "r:\avrtools\appnotes\4414def.inc"
;.include "c:\james\avrtools\appnotes\4414def.inc"
.include "c:\avrtools\appnotes\4414def.inc"
;.include "z:\jdb30\476\avrtools\appnotes\4414def.inc"
;.include "d:\avrtools\avrtools\appnotes\4414def.inc"
.device AT90S8515 ; specifies to the assembler which chip we are using

;port B0 = steering 	-> t1 match a
;port B1 = drive 	-> t1 match b

;port D connected to sensors, serial port
;port A to LED's

;port C to buttons 

;servo max clockwise -> .35ms
;servo max counterclockwise -> 2.15 ms

.def	save 		=R1 	;used to save sreg 
.def	wheelstate	=R2	;state of the brightness sensor on wheel
.def	speed		=R3	;current speed of car
.def	torque		=R4	;current power setting on motor

.def 	speedup		=R5	;flag contining contextual speedup
.def	steernum	=R6	;steering position index

.def	speednum	=r7	;speed position index

.def	speedlag	=r8	;timer to pause between speed changes

.def 	temp 		=R16	;temporary
.def	steerposLo	=R17	;Preload value for timer1 matchA 
.def	steerposHi	=R18	;    "
.def	driveposLo	=R19	;Preload value for timer1 matchB
.def	driveposHi	=R20	;    "
.def	inpin 		=R21	;input pins
.def	count		=R22	;software counter for timer 1
.def	itemp		=R23	;interrupt temp
.def	lastpin		=R24	;last state of input pins
.def	interrupt 	=R25	;timer 1 counter overflow interrupt flag
.def	invert 		=R26	;do we have a white or black line?
.def	stop		=R27	;counter to kill car if we deviate from line for too long
.def	sample		=R28	;sample counter for data sampling
.def	speedctr	=R29	;counter to determine period of wheel rotation

.def	ZL		=R30	;Z registers for memory storage
.def	ZH		=R31

;************************these get re-used in serial stage*********
.def	TXbusy		=r28	;transmit busy flag
.def	RXchar		=r29	;a received character
.def	TXflash		=r27	;is transmit from flash?

;************************reused in wait stage***********************
.def	LED		=r28	;holds status of LED's

;************************timer equates******************************

.equ	t0time 		=20  ;->10ms
.equ	t0preload	=0x06

;************************memory equates*****************************
.equ	firstindex	=0x0067	;first memory index to write
.equ	datastream	=0x0067	;	"
.equ	lastindex	=0x0240	;last memory index to write
.equ	numsamples	=490	;number of samples to take
.equ	samplerate  	=10	;how many samples go by between loggings
;************************serial equates*****************************
.equ	baud96	=25	;9600 baud constant for 4Mhz crystal
.equ	go	='g' 	;0x67 ascii 'g'
.equ	azero	='0'	;0x30 ascii '0'



;************************equates for servo positions****************

.equ	preload		=0xFFFF - 10000

.equ	srvLmax 	=500 * (82 + 0*10)/100 + preload 

.equ	srvL3 		=500 * (82 + 1*10)/100 + preload 

.equ	srvL2 		=500 * (82 + 2*10)/100 + preload

.equ	srvL1 		=500 * (82 + 3*10)/100 + preload 

.equ	srvC 		=500 * (83 + 4*10)/100 + preload 

.equ	srvR1	 	=500 * (83 + 5*10)/100 + preload 

.equ	srvR2	 	=500 * (83 + 6*10)/100 + preload 

.equ	srvR3	 	=500 * (83 + 7*10)/100 + preload 

.equ	srvRmax		=500 * (84 + 8*10)/100 + preload 


;****drive speeds
.equ	neutral		=500 * 109/100 + preload

.equ	brake		=500 * 85/100 + preload

.equ	slow 		=500 * 120/100 + preload
.equ	med 		=500 * 120/100 + preload



.equ	go1		=500 * 112/100 + preload
.equ	go2		=500 * 113/100 + preload
.equ	go3		=500 * 114/100 + preload
.equ	go4		=500 * 115/100 + preload
.equ	go5		=500 * 116/100 + preload





;**********speeds in samples between rotations******
.equ	speedtoolow	= 10 ;less than this, and we consider it sampling noise
.equ	speed5		= 48
.equ	speed4		= 51
.equ	speed3		= 54
.equ	speed2		= 67
.equ	speed1		= 60
.equ	speed0		= 63
.equ	speedtoohigh	= 75 ;greater than this, and we are stopped


;a desired speed:
;10 changes in 2.3 sec
;5 revolutions in 2.3 sec (46)

.equ 	speeduplag	=50;(in ms)	min time to wait between speed adjustments

;************************data segment**********************
.dseg

;define variable strings to be tranmitted from RAM
;datastream: .byte numsamples


;************************code segment*********************
.cseg
.org $0000
	rjmp 	RESET		;reset entry vector
	reti		
	reti
	reti
	rjmp	t1ca		;timer 1 compare a
	rjmp	t1cb		;timer 1 compare b
	rjmp	t1ovfl		;timer 1 overflow
	rjmp	t0ovfl		;timer 0 overflow
	reti
	rjmp	RXdone		;UART receive done
	rjmp	TXempty		;UART buffer empty
	rjmp 	TXdone		;UART transmit done
	reti 	 



;**********************string constants**************
crlf:	.db	0x00, 0x0d, 0x0a, 0x00	;null, carrage return/line feed
;**********************speed setting subroutines***********************

;*************this routine looks at the wheel-turn period, 
		;and translates it into a speed index by lookup.
		;leaves result in temp
_getspeed:
	mov	temp,speedctr		;load the current wheel-turn period
	cpi	temp,speedtoolow
	brsh	nottoosmall
	mov 	temp,speed		;use current speed if time to turn wheel was too small
	ret
	
nottoosmall:	
	cpi	temp,speed5	;Higher speeds here
	brsh	notget5
	ldi 	temp, 5		;period is less than speed5, store a 5
	ret
notget5:
	cpi	temp,speed4
	brsh	notget4
	ldi 	temp, 4
	ret
	
notget4:
	cpi	temp,speed3
	brsh	notget3
	ldi 	temp, 3
	ret
notget3:
	cpi	temp,speed2
	brsh	notget2
	ldi 	temp, 2
	ret
notget2:
	cpi	temp,speed1
	brsh	notget1
	ldi 	temp, 1
	ret
	
notget1:
	cpi	temp,speed0		;slowest speed
	brsh	notget0
	ldi 	temp, 0
	ret
	
notget0:
	ldi 	temp, 0
	ret
;*************************
					;increase speed by 1

_incspeed:
	mov	temp,torque
	inc	temp
	cpi	temp, 6			;make sure we don't go over 5
	brlo	noincovfl
	ldi	temp,5
noincovfl:
	mov	torque,temp		;set new torque
	rjmp	settorque
;*************************

					;decrease speed by one
_decspeed:
	mov	temp,torque
	dec	temp
	cpi	temp,0			;make sure we don't go under 0
	brsh	nodecundfl
	ldi	temp,0
nodecundfl:
	mov	torque,temp		;set new torque
	rjmp	settorque
;*************************
;get current speed, and use it to set the motor ctlr. 

_setspeed:
	rcall	_getspeed	;get current speed index from function call
	mov	speednum,temp	;save current speed index to write to ram
	cp	temp,speed	;temp holds current speed, speed holds desired speed
	breq	settorque	;if we're at our goal speed, don't change
	brlo	_incspeed	;otherwise,speed up or slow down
	rjmp	_decspeed

settorque:			;if speedup is on, we load the torque that is one faster. 
	mov 	temp,speedup	
	cpi 	temp,0xFF
	brne 	loadtorque
	
loadtorqueplus1:
	mov	temp,torque
;	inc	temp
	rjmp 	doneloadtorque
loadtorque:
	mov	temp,torque
doneloadtorque:


	cpi 	temp,0		;from here, we take an desired torque, and set the motor
	brne 	notsp0
	
	ldi	driveposHi, high(neutral)	
	ldi	driveposLo, low(neutral) 	

notsp0:
	cpi	temp,1
	brne	notsp1
	
	ldi	driveposHi, high(go1)	
	ldi	driveposLo, low(go1) 		
	
notsp1:
	cpi	temp,2
	brne	notsp2
	
	ldi	driveposHi, high(go2)	
	ldi	driveposLo, low(go2) 	

	
notsp2:
	cpi	temp,3
	brne	notsp3
	ldi	driveposHi, high(go3)	
	ldi	driveposLo, low(go3) 	
	
notsp3:	
	cpi	temp,4
	brne	notsp4
	ldi	driveposHi, high(go4)	
	ldi	driveposLo, low(go4) 	

notsp4:	
	cpi	temp,5
	brne	notsp5
	ldi	driveposHi, high(go5)	
	ldi	driveposLo, low(go5) 	

notsp5:
	ret


;*******************initial setup*******************
RESET:
	ldi	temp, LOW(RAMEND) 		;setup stack pointer
	out 	SPL, temp
	ldi	temp, HIGH(RAMEND)
	out	SPH, temp
	
	;*****variable init	
	ldi 	steerposLo, low(srvC)		;init servo position to max left
	ldi 	steerposHi, High(srvC)		;

	ldi	driveposHi, high(neutral)	;init motor to neutral
	ldi	driveposLo, low(neutral)		; 	

	clr	invert				;don't invert input by default

	;*******ports
	ser 	temp
	out 	DDRB, temp			;set port b to all outputs (pins 0,1 control servos)

	ser	temp
	out 	DDRA, temp			;set port A to all outputs for LED's
		
	clr 	temp
	out 	DDRD, temp			;set port d to all inputs (sensor array)
	
	clr	temp
	out 	DDRC, temp			;set port C to all inputs (buttons)

	;******TIMSK
	in 	temp, TIMSK
	ori 	temp, 0b11100010		;activate t1 overflow, t1 compare a & b,0,0,0,t0overfl,0 int's
	out 	TIMSK, temp
	
	;******timer0
	ldi 	Temp, 2				;prescale timer 0 by 8
	out 	TCCR0, Temp				
	ldi 	count,t0time

	;******timer1
	;set timer 1 prescale to 1, start timer 1
	;2^16 ticks / 4MHz = 16.384ms between overflows
	in 	temp, TCCR1B
	andi 	temp, 0b11110000
	ori 	temp, 0b00000010		;we just want to set the last nibble
	out 	TCCR1B, temp
	rjmp	waitloop
	
getspeed: rjmp _getspeed
setspeed: rjmp _setspeed
incspeed: rjmp _incspeed
decspeed: rjmp _decspeed


;0******************************get input state & go signal******************
;this phase displays the status of the invert bit in led0.

waitloop:
	ser 	LED			;all LED's off

	mov	temp,	invert 
	com 	temp			;drop in invert bit
	andi	temp,	0b00000001
	com	temp
	and	LED, 	temp
	out	PORTA, 	LED		;display status bits to LEDs
	in 	inpin,  PINC	

	cpi	inpin,0b11111110	;pressing button 0 sets invert
	brne	noinverton
	clr	invert
noinverton:
	cpi	inpin,0b11111101	;pressing button 1 clears invert
	brne	noinvertoff
	ser	invert
noinvertoff:
	cpi	inpin,0b11111011	;pressing 2 starts steering only (desired speed=0)
	brne	nospeed2
	ldi	temp,0
	mov	speed,temp
	rjmp	enterdrive 
nospeed2:
	cpi	inpin,0b11110111	;pressing 3-7 starts car with desired speeds 1-5
	brne	nospeed3
	ldi	temp,1
	mov	speed,temp	
	rjmp	enterdrive
nospeed3:
	cpi	inpin,0b11101111
	brne	nospeed4
	ldi	temp,2
	mov	speed,temp	
	rjmp	enterdrive
nospeed4:
	cpi	inpin,0b11011111
	brne	nospeed5
	ldi	temp,3
	mov	speed,temp	
	rjmp	enterdrive
nospeed5:
	cpi	inpin,0b10111111
	brne	nospeed6
	ldi	temp,4
	mov	speed,temp	
	rjmp	enterdrive
nospeed6:
	cpi	inpin,0b01111111
	brne	nospeed7
	ldi	temp,5
	mov	speed,temp	
	rjmp	enterdrive
nospeed7:
	rjmp waitloop	

	

enterdrive:					;get ready to drive car
	;******memory
	ldi 	ZL,low(datastream)		;set memory base index
	ldi	ZH,high(datastream)
	;*********variable init
	ldi 	lastpin, 0b11111111		;init inpins
	ldi	sample, samplerate		;init samplerate
	clr	temp
	mov	torque,temp			;init torque to 0
	mov	speedup, temp			;init speed boost to off
	mov	speednum,temp			;init initial speed index to 0
	ldi	temp,5				
	mov	steernum,temp			;start steering indexer at "centered"
	ldi	temp,speeduplag	
	mov	speedlag,temp			
	
	;********get state of wheel
	in 	inpin,PIND
	ori	inpin,0b00000100		;mask out speed feedback bit
	cpi	inpin,0b00000100
						;initalize state of wheel sensor
	clr	temp
	mov	wheelstate,temp
						;set wheelstate to 0 (sensor sees black band)
	breq	white
	ser 	temp
	mov	wheelstate,temp			;set it to FF if wheel is black
white:

	
	
	
	sei					;enable interrupts	
	clr stop				;clear line loss stop counter
;0*****************************drive car and get data************************
loop:	ser 	temp			;on interrupt, break out of loop
	cp 	temp,interrupt
	brne 	loop
	
	clr	interrupt		;reset interrupt flag
	
	
;**********deal with car speed	
	inc	speedctr		;inc wheel period counter

	mov	temp, speedlag
	cpi	temp,100		;see if speedlag has overflowed (if so, we can change
	brlo	donewheel		;speeds again)

	ldi	temp,200		;once we have overflowed, hold lag at 200 (>100)
	mov	speedlag,temp

	cpi	speedctr,speedtoohigh	;if we haven't moved in a long time, invoke speedup
	breq	dospeedupdatebctooslow
	
	mov	temp,wheelstate
	in	inpin, PIND
	andi	inpin, 0b00000100	;get wheel feedback bit
	cpi	inpin, 0b00000100
	breq	checkwheeltrue

checkwheelfalse:			;check for low to high transitions
	cpi	temp, 255		;wheel sensor on white
	breq	wheelstatechangedupdate
	rjmp 	donewheel
checkwheeltrue: 
	cpi	temp,0			;wheel sensor on black
	breq	wheelstatechanged
	rjmp	donewheel
dospeedupdatebctooslow:			;we weren't moving, so speed up
	rcall	setspeed
	clr	speedctr		;reset wheel period counter
	ldi	temp,speeduplag		;reset lag counter so we don't speed up again too soon
	mov	speedlag,temp
	rjmp 	donewheel

wheelstatechangedupdate:
	rcall	setspeed		;we had a low to high transition on the wheel sensor. 
					;time to update torque, speed
	clr	speedctr		;start period counter again
	
	ldi	temp,speeduplag		;don't change speeds again too soon.
	mov	speedlag,temp
wheelstatechanged:
	com	wheelstate
donewheel:
	dec	speedlag

;*********deal with steering***********
	mov 	lastpin, inpin		;back-up last reading
	in 	inpin, PIND		;get sensor data



	cpi invert,0
	breq noinvert
	com 	inpin			;invert(if user requested it)
noinvert:


	andi 	inpin, 0b11111000	;ignore low 3 bits
	ori	inpin, 0b00000111	;we'll need them for the serial port & speed

	out 	PORTA, stop		;send sensors to LEDs	

	rcall 	srvcase			;get sensor reading & set servoindex accordingly


	ldi	temp, high(datastream+numsamples)	;if we are out of memory,
	cpi	ZH,low(datastream+numsamples)
	cpc	ZL,temp
	brlo 	loopexitconds				;don't store anything

	dec	sample					;we only record every samplerate th entry
	cpi	sample, 0				;record smaples when "sample" reg. underflows
	brne	loopexitconds
	ldi	sample, samplerate

	mov	temp, steernum				;store steering in high nibble
	swap	temp					
	andi	temp, 0b11110000			;store speed in low nibble
	or	temp, speednum
	st	Z+, temp				;write byte into ram



loopexitconds:
	sbis	PINC,0			;break out of loop on pin 7
	rjmp enddrive

	cpi stop, 50
	brsh enddrive			;break out of loop if line lost


	rjmp loop	

enddrive:
;***********cleanup from drive

	ldi	temp, 0
	st	Z+, temp			;null terminate the data stream in memory
	;display checkerboard on led's
	ldi	temp,0b01010101
	out	PORTA,temp

	;**********apply brakes for 1 sec	
	ldi	driveposLo,low(brake)	
	ldi	driveposHi,high(brake)

	ldi 	inpin, 255-100			;use inpin as a counter
loopstop:	
	ser 	temp				;on interrupt,inc soft counter
	cp 	temp,interrupt			
	brne 	loopstop
	
	clr	interrupt
	
	inc	inpin				;inc counter
	breq	donebraking			;finish on overflow
	rjmp 	loopstop

donebraking:


	cli				;turn off all interrupts

	clr temp
	out TIMSK, temp			;turn off all timers


;0****************************wait for g on serial port and download***************

	clr	TXbusy		;start out not busy on TX
	ldi	RXchar, go	;send all of the data immediately


	;setup UART -- enable TXempty & RXdone int, and RX, TX pins
	ldi 	temp, 0b10111000
	out 	UCR, temp
	;set baud rate to 9600
	ldi	temp, baud96
	out	UBRR, temp
	;intialize text pointer BEFORE turning on interrupts
	;because RESET causes the TX empty flag to be SET
	ldi	ZL, LOW(crlf<<1) ;do shift to convert word-addr to byte
	ldi 	ZH, HIGH(crlf<<1)

	sei

;******
;now dump data whenever we get a g key on the serial port
TXloop:	cpi 	RXchar, go	;wait for go signal - a 'g' on the keyboard
	brne	TXloop

	
	;now the pointer to the variable message in RAM
	ldi	ZL, LOW(datastream)  ;ptr to RAM
	ldi	ZH, HIGH(datastream) 
	ld	r0,Z
	out	UDR, r0		;fire off the UART transmit
	clr	TXflash		;the string is in RAM
	ser	TXbusy		;and set the TX busy flag
	sbi	UCR, UDRIE	;enable the TXempty interrupt
	rcall	TXwait
	
					;send a null,cr,lf,null to end sequence
	;setup ptr for <crlf> string
	ldi	ZL, LOW(crlf<<1)  ;shifted becuase pgm memory is words
	ldi	ZH, HIGH(crlf<<1)
	lpm
	out	UDR, r0		;trigger the UART TX
	ser	TXflash		;text string in flash memory
	ser	TXbusy		;and set the TX busy flag
	sbi	UCR, UDRIE	;enable the TXempty interrupt
	rcall	TXwait

	clr RXchar
	rjmp	TXloop




	
	
				
;*******************************sensor reading case statement********************


;note: in the code, these binary numbers are the mirror of what the car sees.

srvcase:
	cpi	inpin,0b11110111
	brne	notc4			;about to lose line. steer left hard

	clr 	stop			;we see the line again, so we don't need to stop.

	ldi 	steerposLo,low(srvLmax)	;put timer1match vaules in registers
	ldi 	steerposHi,high(srvLmax)
	
	ldi	temp, 1 + 1		;matlab will see this as a 1	
	mov	steernum, temp
	
	ret

notc4:	
	cpi	inpin,0b11100111
	brne	qnotc4			;steer left 2

	clr 	stop		

	ldi 	steerposLo,low(srvL3)
	ldi 	steerposHi,high(srvL3)

	ldi	temp, 2 + 1
	mov	steernum, temp

	ret
	
qnotc4:	
	cpi	inpin,0b11101111
	brne	anotd4			;steer left 1

	clr 	stop

	ldi 	steerposLo,low(srvL2)
	ldi 	steerposHi,high(srvL2)

	ldi	temp, 3 + 1
	mov	steernum, temp

	ret

anotd4:	
	cpi	inpin,0b11001111
	brne	inpin4			;just left of center.

	clr 	stop

	ldi 	steerposLo,low(srvL1)
	ldi 	steerposHi,high(srvL1)

	ldi	temp, 4 + 1
	mov	steernum, temp

	ret

inpin4:	
	cpi	inpin,0b11011111	;dead center.
	brne	anotf4

	clr 	stop

	ldi 	steerposLo,low(srvC)
	ldi 	steerposHi,high(srvC)

	ldi	temp, 5 + 1
	mov	steernum, temp

	ret



anotf4:	
	cpi	inpin,0b10011111
	brne	notf4			;just right of center	

	clr 	stop

	ldi 	steerposLo,low(srvR1)
	ldi 	steerposHi,high(srvR1)

	ldi	temp, 6 + 1
	mov	steernum, temp

	ret

notf4:	
	cpi	inpin,0b10111111
	brne	rnotf4			;steer right 1

	clr 	stop

	ldi 	steerposLo,low(srvR2)
	ldi 	steerposHi,high(srvR2)

	ldi	temp, 7 + 1
	mov	steernum, temp

	ret

rnotf4:	
	cpi	inpin,0b00111111
	brne	notg4			;steer right 2

	clr 	stop

	ldi 	steerposLo,low(srvR3)
	ldi 	steerposHi,high(srvR3)

	ldi	temp, 8 + 1
	mov	steernum, temp

	ret
	
notg4:	
	cpi	inpin,0b01111111
	brne	nota4
			;about to lose line. steer right hard
	clr 	stop

	ldi 	steerposLo,low(srvRmax)
	ldi 	steerposHi,high(srvRmax)

	ldi	temp, 9 + 1
	mov	steernum, temp

	ret



;these cases do not change steering

nota4:	cpi	inpin,0b00011111		;slow down
	brne	xnota4				;this reading turns off speed boost
	clr	temp
	mov	speedup,temp

	ret
xnota4:
	cpi	inpin,0b11000111		;speed up
	brne	znota4				;this reading turns on speed boost
	ser	temp
	mov	speedup,temp

	ret			
	
znota4:						
	cpi	inpin,0b11111111		;we can't see the line.
	brne	notb4				;maintain previous heading
			
	inc 	stop				;when stop reaches 50, we will stop.
	
	ldi	temp, 0 + 1
	mov	steernum, temp

	ret
notb4:		
	ldi	temp, 0 + 1			;default case
	mov	steernum, temp			;maintain heading
	ret




nowrite:
	ret


****************************interrupt routines*****************************

; UART needs a character
TXempty:in	save, SREG	;save processor status
	tst	TXflash		;Is the string in flash memory?
	breq	TXram		;If not, it is in RAM
	inc	ZL		;get the next char from flash			
	lpm			;and put it in r0
	rjmp	TXfls
TXram: 	inc	ZL		;get the next char from RAM
	ld	r0,Z
TXfls:	tst	r0		;if char is zero then exit
	breq	TXend		
	out 	UDR, r0		;otherwise transmit it
	rjmp	TXexit		;exit until next char
TXend:	clr	TXbusy		;no more chars
	cbi	UCR, UDRIE	;clear the TXempty interrupt
TXexit:	out	SREG, save	;restore proc status
	reti			;back to pgm

; TX done -- buffer is empty  -- unused here
TXdone:	in	save, SREG	;save processor status
	out	SREG, save	;restore proc status
	reti			;back to pgm

; UART read a character
RXdone:	in	save, SREG	;save processor status	
	in	RXchar, UDR	;get the character
	out	SREG, save	;restore proc status
	reti			;back to pgm
;**********************Timer 1 overflow**************
;brings two servo lines hi, reloads servo position values
;
;_______|-|____
;      /\
;       |
;	overflow here
t1ovfl:
	in save, SREG			;save status reg
	sbi 	portb,0			;set steering servo line hi
	sbi 	portb,1			;set drive servo line hi
	
	ldi 	temp, high(preload)
	out	TCNT1H, temp
	ldi	temp, low(preload)
	out	TCNT1L, temp

	out 	OCR1AH, steerposHi	;load steering position for next sample
	out 	OCR1AL, steerposLo
	out 	OCR1BH, driveposHi	;load drive positon for next sample
	out 	OCR1BL, driveposLo

	out 	SREG, save		;restore status register
	reti

;********************Timer0 overflow*****************
t0ovfl:
	in	save, SREG
	
	dec	count			;keep track of # of intr
	brne	notyet			;=100 millsec? then:
	
	ser	interrupt		;signal code to scan
	
	ldi	count, t0time	    	;reset count for next 100 mSec period
notyet:	ldi	itemp, t0preload	;preload timer=250 counts till overflow
	out 	TCNT0, itemp		;250x8x.25 microSec = 0.5mSec.

	
	out 	SREG, save
	reti
;********************Timer 1 compare A***************
;drops steering servo line low after correct delay
t1ca:
	in 	save, SREG		;save status reg
	cbi 	portb,0			;set steering servo line low
	out 	SREG, save		;restore status reg
	reti

;********************Timer 1 compare B***************
;drops drive servo line low after correct delay
t1cb:
	in 	save, SREG		;save status reg
	cbi	portb,1			;set drive servo line low
	out	SREG,save		;restore status reg
	reti


;*****************************
;subroutine

TXwait:	tst	TXbusy		;now wait for the tranmission to finish
	brne	TXwait
	ret