'---------------------------------------------------------------------------
' serservo2.txt
'---------------------------------------------------------------------------
'Programmer : 	Jim McCarthy, Tom Ogden
'
'File: 		serservo2.txt
'
'Date: 		03-29-03
'
'Assignment: 	Project 1
'
'Course: 	Robotics I -- 91.548-201
'
'Descr.:	This is the source code that runs on the basic stamp that
'		controls the servos that position the camera.  The Basic Stamp
'		runs an interpreter that converts this code into assembly 
'		code for the Basic Stamp.
'
'		This is the latest release of the program on the first step controller 
' 		used to control the servo driven webcam
'
' 		notice that the serin data is inverted (via N2400 - instead of T2400)
' 		this is because we're using the simple 22k resistor to interface w/
' 		the pc, and that will be read inverted
'
' 		enhancements:
' 		smoother transition when speed > 0
' 		power on self test & set to center position
' 		uses eeprom to store lastposition for each servo 0-3
'
'---------------------------------------------------------------------------


symbol servo = b3
symbol pos = b4
symbol speed = b5
symbol x = b7
symbol lastpos = b8

output 6
input 7

  pause 400

' set servo to center
  for servo = 0 to 3
    for x = 1 to 30
      pulsout servo,150
      pause 30
    next x
  write servo, 150
  next servo

start:

  serin 7,N2400,servo,pos,speed

  ' get lastpos from eeprom (eeprom location 0-3 corresponds to servo 0-3)
  read servo, lastpos

'no relative positioning support yet
  'if mode < 1 then directpos
  ' when mode >= 1, do relative positioning and increment lastpos by pos
  'pos = lastpos + pos

directpos:
  if pos < 90 then set90
  if pos > 210 then set210
  goto calcspeed

' if pos is < minboundary, set to minboundary
set90:
  pos = 90
  goto calcspeed

' if pos is > maxboundary, set to maxboundary
set210:
  pos = 210

calcspeed:
  if speed >= 0 then speedchk2
  speed = 0
speedchk2:
  if speed <= 50 then dospeedpulse
  speed = 50

dospeedpulse:
  if lastpos >= pos then backwards

forwards:
  'debug "forwards"
  'debug speed
  for x = lastpos to pos step 1
    pulsout servo,x
    pause speed
  next x 
  goto savepos

backwards:
  'debug "backwards"
  'debug speed
  for x = lastpos to pos step -1
    pulsout servo,x
    pause speed
  next x 
  goto savepos

savepos:
  if servo > 3 then start
  write servo, pos
  goto start