' PROGRAM: Servo.bas

' Taken from Parallax Stamp Application Note #4: Controlling & Testing Servos

' July 28, 1993

' The Stamp works as a servo test bench. It provides a cycling servo signal 
' for testing, and measures the pulse width of external servo signals. 

Symbol	E	=	5	' Enable pin, 1 = enabled
Symbol	RS	=	4	' Register select pin, 0 = instruction
Symbol	char	=	b0	' Character sent to LCD. 
Symbol	huns	=	b3	' BCD hundreds
Symbol	tens	=	b6	' BCD tens
Symbol	ones	= 	b7	' BCD ones
Symbol	BCDin	=	b8	' Input to BCD conversion/display routine.
Symbol	buttn	=	b9	' Button workspace
Symbol	i	=	b10	' Index counter

' Load text strings into EEPROM at address 0. These will be used to display 
' status messages on the LCD screen.

EEPROM 0,("Cycling... Pulse Width: ")

' Set up the Stamp's I/O lines and initialize the  LCD. 

begin:	let pins = 0		' Clear the output lines
	let dirs = %01111111		' One input, 7 outputs. 
	pause 200			' Wait 200 ms for LCD to reset. 

' Initialize the LCD in accordance with Hitachi's instructions
' for 4-bit interface. 

i_LCD:	let pins = %00000011		' Set to 8-bit operation. 
	pulsout E,1		' Send above data three times
	pause 10			' to initialize LCD. 
	pulsout E,1		
	pulsout E,1		
	let pins = %00000010		' Set to 4-bit operation. 
	pulsout E,1		' Send above data three times. 
	pulsout E,1
	pulsout E,1
	let char = 12		' Set up LCD in accordance w/
	gosub wr_LCD		' Hitachi instruction manual. 
	let char = 6		' Turn off cursor, enable
	gosub wr_LCD		' left-to-right printing.  
	high RS			' Prepare to send characters. 

' Measure the width of input pulses and display on the LCD. 

mPulse:	output 3
	gosub clear		' Clear the display.
	for i = 11 to 23		' Read "Pulse Width:" label
	  read i, char
	  gosub wr_LCD		' Print to display
	next
	pulsin 7, 1, BCDin		' Get pulse width in 10 us units.
	gosub BCD			' Convert to BCD and display. 
	pause 500
	input 3			' Check button; cycle if down. 
	button 3,1,255,10,buttn,1,cycle
	goto mPulse		' Otherwise, continue measuring. 

' Write the ASCII character in b3 to LCD. 

wr_LCD:	let pins = pins & %00010000
	let b2 = char/16		' Put high nibble of b3 into b2. 
	let pins = pins | b2		' OR the contents of b2 into pins. 
	pulsout E,1		' Blip enable pin. 
	let b2 = char & %00001111	' Put low nibble of b3 into b2. 
	let pins = pins & %00010000	' Clear 4-bit data bus. 
	let pins = pins | b2		' OR the contents of b2 into pins.
	pulsout E,1		' Blip enable. 
	return

clear:	low RS			' Change to instruction register. 
	let char = 1		' Clear display. 
	gosub wr_LCD		' Write instruction to LCD. 
	high RS			' Put RS back in character mode. 
	return

' Convert a byte into three ASCII digits and display them on the LCD.  
' ASCII 48 is zero, so the routine adds 48 to each digit for display on the LCD

BCD:	let huns= BCDin/100+48	' How many hundreds?
	let tens= BCDin//100		' Remainder of #/100 = tens+ones.
	let ones= tens//10+48	' Remainder of (tens+ones)/10 = ones. 
	let tens= tens/10+48		' How many tens?
	let char= huns		' Display three calculated digits. 
	gosub wr_LCD
	let char = tens
	gosub wr_LCD
	let char = ones
	gosub wr_LCD
	return

' Cycle the servo back and forth between 0 and 90 degrees. Servo moves slowly ' in one direction (because of 20-ms delay between changes in pulse width) and quickly ' in the other. Helps diagnose stuck servos, dirty feedback pots, etc. 

cycle:	output 3
	gosub clear
	for i = 0 to 9		' Get "Cycling..." string and 
	  read i, char		' display it on LCD. 
	  gosub wr_LCD
	next i
reseti:	let i = 100		' 1 ms pulse width. 
cyloop:	pulsout 6,i		' Send servo pulse. 
	pause 20			' Wait 1/50th second.
	let i = i + 2		' Move servo.
	if i > 200 then reseti	' Swing servo back to start position. 
	input 3			' Check the button; change function if down.
	button 3,1,255,10,buttn,1,mPulse
	goto cyloop		' Otherwise, keep cycling.