' {$STAMP BS2}
' {$PBASIC 2.5}

serStr VAR Byte(8)
I VAR Byte
P VAR Word
D VAR Byte

Init:
FOR I = 0 TO 15
  LOW I
NEXT
D = 0

HIGH 0

DEBUG "Basic Stamp 2 startup",CR
SEROUT 15, 84, ["Basic Stamp 2 startup",CR]

Read_Loop:
serStr(0) = 0
serStr(8) = 0

HIGH 0
SERIN 14, 84, 10000, No_Data, [STR serStr\8\"#"]
LOW 0

' Debug mode display input command on debug terminal
IF D = 1 THEN
  FOR i = 0 TO 5
  IF serStr(i) > 0 THEN
    DEBUG serStr(I)
  ENDIF
  NEXT
  DEBUG CR,LF
ENDIF

SELECT serStr(1)
  ' Initialize basic stamp 2
  CASE "I"
    FOR I = 0 TO 13
      LOW I
    NEXT
    SEROUT 15, 84, ["Initialize pins 0-13 LOW", CR]

  ' Return verison and date
  CASE "V"
    SEROUT 15, 84, ["AutoGuider and Focuser Control v2.0",CR,LF,"  30-Jan-2012", CR]

  ' Debug mode
  CASE "D"
  SELECT serStr(2)
    CASE "1"
    D = 1
    SEROUT 15, 84, ["Debug mode On", CR]
    CASE ELSE
    D = 0
    SEROUT 15, 84, ["Debug mode Off", CR]
  ENDSELECT

  ' Autoguider control
  ' start guide
  CASE "M"
  SELECT serStr(2)
    CASE "n"
      SEROUT 15, 84, ["Guide north",CR]
      LOW 3
      HIGH 2
    CASE "s"
      SEROUT 15, 84, ["Guide south",CR]
      LOW 2
      HIGH 3
    CASE "e"
      SEROUT 15, 84, ["Guide east",CR]
      LOW 4
      HIGH 1
    CASE "w"
      SEROUT 15, 84, ["Guide west",CR]
      LOW 1
      HIGH 4
  ENDSELECT

  ' stop guide
  CASE "Q"
  SELECT serStr(2)
    CASE "n"
      SEROUT 15, 84, ["Stop north",CR]
      LOW 2
    CASE "s"
      SEROUT 15, 84, ["Stop south",CR]
      LOW 3
    CASE "e"
      SEROUT 15, 84, ["Stop east",CR]
      LOW 1
    CASE "w"
      SEROUT 15, 84, ["Stop west",CR]
      LOW 4
    CASE ELSE
      SEROUT 15, 84, ["Stop all",CR]
      LOW 1
      LOW 2
      LOW 3
      LOW 4
  ENDSELECT

  ' Focus motor
  CASE "F"
  SELECT serStr(2)
    CASE "+"
      SELECT serStr(3)
        CASE 0
          SEROUT 15, 84, ["Focus 1 out",CR]
          LOW 9
          HIGH 8
          LOW 5
          HIGH 9
        CASE ELSE
          SEROUT 15, 84, ["Focus 1 small out",CR]
          GOSUB AsciiToDecimal
          LOW 9
          HIGH 8
          LOW 5
          HIGH 9
          PAUSE (P)
          LOW 9
      ENDSELECT
    CASE "-"
      SELECT serStr(3)
        CASE 0
          SEROUT 15, 84, ["Focus 1 in",CR]
          LOW 9
          LOW 8
          HIGH 5
          HIGH 9
        CASE ELSE
          SEROUT 15, 84, ["Focus 1 small in",CR]
          GOSUB AsciiToDecimal
          LOW 9
          LOW 8
          HIGH 5
          HIGH 9
          PAUSE (P)
          LOW 9
      ENDSELECT
    CASE "Q"
      SEROUT 15, 84, ["Focus 1 stop",CR]
      LOW 9
  ENDSELECT

  ' Focus motor 2
  CASE "O"
  SELECT serStr(2)
    CASE "+"
      SELECT serStr(3)
        CASE 0
          SEROUT 15, 84, ["Focus 2 out",CR]
          LOW 6
          HIGH 10
          LOW 11
          HIGH 6
        CASE ELSE
          SEROUT 15, 84, ["Focus 2 small out",CR]
          GOSUB AsciiToDecimal
          LOW 6
          HIGH 10
          LOW 11
          HIGH 6
          PAUSE (P)
          LOW 6
      ENDSELECT
    CASE "-"
      SELECT serStr(3)
        CASE 0
          SEROUT 15, 84, ["Focus 2 in",CR]
          LOW 6
          LOW 10
          HIGH 11
          HIGH 6
        CASE ELSE
          SEROUT 15, 84, ["Focus 2 small in",CR]
          GOSUB AsciiToDecimal
          LOW 6
          LOW 10
          HIGH 11
          HIGH 6
          PAUSE (P)
          LOW 6
      ENDSELECT
    CASE "Q"
      SEROUT 15, 84, ["Focus 2 stop ",CR]
      LOW 6
  ENDSELECT
ENDSELECT

GOTO Read_Loop
STOP

AsciiToDecimal: P = 0
FOR I = 3 TO 5
IF serStr(I) = 32 THEN NextFor
P = P * 10 + (serStr(I) - "0")
NextFor:
NEXT
RETURN

No_Data:
GOTO Read_Loop
SEROUT 15, 84, ["Serial data read timeout" ,CR,LF]
STOP