' {$STAMP BS2}
' {$PBASIC 2.5}
' **************************************
' * Robot network control.             *
' * Electronic Systems Technology Club *
' * Southern Illinois University       *
' * www.siu.edu/~isat/est              *
' **************************************
myAddr    CON $1     ' Node Address ******* change for each ********
DestAddr  CON $0     ' Destination address

Baud      CON 84     ' Baud rate, 9600, 8-N-1, non-inverted, on BS2.

RX        PIN 0      ' Receive Pin
TX        PIN 2      ' Transmit Pin
RTS       PIN 6      ' Flow control Pin

Left      PIN 12     ' Servo
Right     PIN 13     ' Servo

IrRight   PIN 1
IrLeft    PIN 9

IrLEDRight  PIN 2
IrLEDLeft   PIN 8

Action VAR Byte

irDetectLeft VAR Word
irDetectRight VAR Word
datain  VAR Byte
x VAR Word

HIGH TX

  SEROUT tx,84,[DEC myaddr,", Configurating XBee.. ", datain,CR]
PAUSE 2000                               ' Guard time for command sequence
SEROUT TX,Baud,["+++"]                   ' Enter command mode
PAUSE 2000                               ' Guard time for command sequence
SEROUT TX,Baud,["ATMY ", HEX myAddr,CR,  ' Set node address
               "ATDL ", HEX DestAddr,CR, ' Set destination node address
               "ATD6 1",CR,              ' Use RTS for flow control
               "ATRO 50",CR,             ' Increase packet timeout to keep data together
               "ATCN",CR]                ' Exit command mode

PAUSE 1000
  SEROUT tx,84,[DEC myaddr,", Configuration Complete! ", datain,CR]
DO
  SERIN rx\RTS,84,2,timeout,[datain]     ' Accept data
  SELECT datain
   CASE "F"                              ' Report back action
      SEROUT tx,84,[DEC myaddr,",Going Forward! ",CR]
   CASE "S"
      SEROUT tx,84,[DEC myaddr,",I'm stopped! ",CR]
   CASE "L"
      SEROUT tx,84,[DEC myaddr,",Moving left! ",CR]
   CASE "R"
      SEROUT tx,84,[DEC myaddr,",Moving right! ",CR]
   CASE "B"
      SEROUT tx,84,[DEC myaddr,",Backing up! ",CR]
   CASE "A"
      SEROUT tx,84,[DEC myaddr,",Auto mode! ",CR]
  ENDSELECT


timeout:                                 ' Take action
SELECT datain

   CASE "F"
      GOSUB Forward
   CASE "S"
      PAUSE 2
   CASE "L"
      GOSUB TurnLeft
   CASE "B"
      GOSUB Backwards
   CASE "R"
      GOSUB TurnRight
   CASE "A"
      FREQOUT IrLEDLeft, 1, 38500
      irDetectLeft = IrLeft
      FREQOUT IrLEDRight, 1, 38500
      irDetectRight = IrRight
    IF (irDetectLeft = 0) AND (irDetectRight = 0) THEN
      IF Action <> "B" THEN SEROUT Tx, 84, [DEC myaddr,",Auto: Back", CR]
      Action ="B"
      FOR x = 1 TO 20
        GOSUB Backwards
      NEXT
    ELSEIF (irDetectLeft = 0) THEN
      IF Action <> "L" THEN SEROUT Tx, 84, [DEC myaddr,",Auto: Left", CR]
      Action = "L"
      GOSUB TurnLeft
    ELSEIF (irDetectRight = 0) THEN
      IF Action <> "R" THEN SEROUT Tx, 84, [DEC myaddr,",Auto: Right", CR]
      Action = "R"
      GOSUB TurnRight
    ELSE
      IF Action <> "F" THEN SEROUT Tx, 84, [DEC myaddr,",Auto: Forward ", CR]
      Action = "F"
      GOSUB Forward
      ENDIF
      ENDSELECT
PAUSE 15
LOOP

Forward:                                ' Move routines
  PULSOUT left, 1000
  PULSOUT right, 500
  RETURN
TurnLeft:
  PULSOUT left, 900
  PULSOUT right,900
  RETURN
TurnRight:
  PULSOUT left, 600
  PULSOUT right,600
  RETURN
Backwards:
  PULSOUT left, 500
  PULSOUT right,1000
  RETURN