'WIFLY BEE REMOTE CONTROL DEMO==================
'
'   {$STAMP BS2}
'   {$PBASIC 2.5}
'
' =========================================================================


' -----[ Program Description ]-------------
' Updated 6/23/2012 by R.Kessler
' This program is meant to drive a BOE-Bot by receiving commands from a
' WIFLY Bee RN-XV wireless radio from SparkFun.  The commands are simple ascii characters,
' the charachters on the keyboard are used to control
' how the BOE-Bot drives. This is slightly modified from the original XBEE program
'so the bot will move farther with each command.
'To Run:
'Configure Wifly for Open Wireless, Channel 6 and the ssid of your router. Set wlan join =1 so
'the module will always join your Wifi lan when it boots.

'This program communicates to the Boe via a Parallax SIP XBEE adapter and no changes in the wiring
'from the XBEE is necessary! Just exchange XBEE and WiFly modules as desired!

'Wiring the SIP adapter. +5 =====> Vdd
'                        GND=====> Vss
'                        DOUT====> Pin 2 on BOE   My program listens for commans on this line
'                        DIN=====> Pin 1 on BOE
'                        RTS=====> Pin 11 on BOE (not used but connected)

'Use Tera Term to control the bot via a TCP/IP connection. Use 192.168.1.102 2000 for example.
'This will communicate with the modem through port 2000 (the default). You just need to find out what
'the IP of your Wifly is. If you created a static IP then you will use that, of course.
'Once connected in Tera Term, choose Setup | Terminal and click on local Echo so you can see the commands
'you type. Also select CR+LF for Receive and send to make your commands show up on a new line.
' Press F, B, L, OR R THEN ENTER TO move the bot!


' Pin definitions

Baud_Mode      CON     84   '9600 8 bit, no parity  Int(1,000000/9600)-2 = 84

XBEE_TX        PIN  1      'DIN pin on XBee module
XBEE_RX        PIN  2      'DOUT pin on XBee module

LEFT_SERVO     PIN  13
RIGHT_SERVO    PIN  12
'PING_SERVO     PIN  14


L_FWD     CON  1000
L_REV     CON  500
L_STOP    CON  750
R_FWD     CON  500
R_REV     CON  1000
R_STOP    CON  750
'SCALE     CON  $200    'hex for raw * 2us pulse for BS2



' -----Define Variables------------

i         VAR  Byte
rx_data   VAR  Byte



'Initialize motors to off

  LOW LEFT_SERVO
  LOW RIGHT_SERVO



' -----Main loop------

Main:


  DO
    SERIN XBEE_RX, BAUD_MODE, [STR rx_data\1] 'get data from the WiFly

    DEBUG "Received data: ", STR rx_data\1

    SELECT rx_data
      CASE $6C     ' ascii for 'l'
        GOSUB Turn_left

      CASE $62     ' ascii for 'b'
        GOSUB Backup

      CASE $72     ' ascii for 'r'
        GOSUB Turn_right

      CASE $66     ' ascii for 'f'
        GOSUB Forward

      ENDSELECT


  LOOP


  END


Forward:
  DEBUG "Forward", CR
  FOR i = 0 TO 50
    PULSOUT LEFT_SERVO, L_FWD
    PULSOUT RIGHT_SERVO, R_FWD
    PAUSE 20
  NEXT

  RETURN

Backup:
  FOR i = 0 TO 50
    PULSOUT LEFT_SERVO, L_REV
    PULSOUT RIGHT_SERVO, R_REV
    PAUSE 20
  NEXT

  RETURN

Turn_left:
  FOR i = 0 TO 50
    PULSOUT LEFT_SERVO, L_STOP
    PULSOUT RIGHT_SERVO, R_FWD
    PAUSE 20
  NEXT

  RETURN

Turn_right:
  FOR i = 0 TO 50
    PULSOUT LEFT_SERVO, L_FWD
    PULSOUT RIGHT_SERVO, R_STOP
    PAUSE 20
  NEXT

  RETURN