' {$STAMP BS2}
' {$PBASIC 2.5}
' Tracker Bot Webcam Controller
' By Dayhkr 2009
' http://www.digital-domain.us

Cam_Dir  VAR  Byte

#Not using right now until it works
'Define Position Variables
'L_Signal PIN 5
'C_Signal PIN 4
'R_Signal PIN 3

LOW 6  'Motor Controller reset
HIGH 6
PAUSE 30

'Set Pins to input
INPUT 3  'Cam position Right
INPUT 4  'Cam position Center
INPUT 5  'Cam position Left

LOW 5
LOW 3
LOW 4

Cam_Start:
GOSUB Cam_Left
PAUSE 2000
GOSUB Cam_Right
PAUSE 2000
GOSUB Cam_Center
END

'commented out until I can get the board to work
'DO
  'DEBUG CLS, "Enter Cam Direction: "
  'DEBUGIN Cam_Dir
  'SELECT Cam_Dir
   ' CASE "1"
    '  GOSUB Cam_Left
    'CASE "2"
    '  GOSUB Cam_Right
    'CASE "3"
      'GOSUB Cam_Center
  'ENDSELECT
  'PAUSE 10
'LOOP

Cam_Left:
  IF (IN5 = 0) THEN
    'DEBUG "y"
    SEROUT 7, 84,[$80, 0, 1, 25]
    DO
      IF(IN5 = 1) THEN EXIT
    LOOP
    SEROUT 7, 84,[$80, 0, 1, 0]
    RETURN
  ENDIF
END

Cam_Right:
  IF (IN3 = 0) THEN
    SEROUT 7, 84,[$80, 0, 0, 25]
    DO
      IF(IN3 = 1) THEN EXIT
    LOOP
    SEROUT 7, 84,[$80, 0, 0, 0]
    RETURN
  ENDIF
END

Cam_Center:
  IF (IN4 = 0 AND IN3 = 1) THEN
    SEROUT 7, 84,[$80, 0, 1, 25]
    DO
      IF(IN4 = 1) THEN EXIT
    LOOP
    SEROUT 7, 84,[$80, 0, 1, 0]
  RETURN
  ELSEIF (IN4 = 0 AND IN5 = 1) THEN
    SEROUT 7, 84,[$80, 0, 0, 20]
    DO
      IF(IN4 = 1) THEN EXIT
    LOOP
    SEROUT 7, 84,[$80, 0, 0, 0]
    RETURN
  ENDIF
END

 