BasicMicro - Forums

=========================================================================
'
' File:       HE_BS2_COMMS_3.bs2
' Purpose:    Using SFR08 to control HexEngine, VIA BS2p
' Author:     Matt Denton
' Updated:    27/08/08
'
' {$STAMP BS2p}
' {$PBASIC 2.5}
'
' =========================================================================
' -----[ Program Description ]---------------------------------------------
'
' This programs demonstrates two way serial comunications betwwen the
' HexEgnine and a basic stamp BS2p device using the SERIN and SEROUT
' commands. In order to get this to work when using SERIN, the maximum
' baud rate is 19200, and a delay is needed from when the HexEngine receives
' a packet request to when it returns it. This is because we are not using
' hardware uarts on the BS2p. There is a setting on the HexEngine that
' will transmit a number of 0xff bytes to form a transmision delay
' before the actual reply packet. This setting also sets the UART STOP
' bits TO two, which also helps slow down the comms data rate.
' Also, it is much easier To receive PIP packets in mode 0, with
' no Escape code checking, Therfore before running this test set the
' following in the HexEngine configuration menu:
' CBR=4
' TXD=3
' PIP=0
' This sets the baud rate to 19200, transmission delay on and PIP mode 0
' Don't forget to exit config mode when you are done!
'
' Only 3 wires are needed for comunication with the BS2p, which can be
' found on the p.Brain-SMB connector CN17.
' Pin 2 = U1RX
' Pin 4 = U1TX,
' Pin 6 = GROUND
'
' U1RX needs to be connecteed to the BS2p SER_OUT pin, U1TX to BS2p SER_IN
' and GROUND to the BS2p ground. If you are using the 5V regulator on the
' p.Brain-SMB, You can also take a fourth wire from Pin 9 on CN17 to
' supply the 5V power for the BS2p device.
'
' [   HexEngine   ]           [ BS2p    ]
' [        U1TX   ]----TX---->[ SER_IN  ]
' [        U1RX   ]<---RX-----[ SER_OUT ]
' [        GROUND ]----GND----[ GROUND  ]
' [        5V     ]- - 5V- - -[ 5V      ]
' [               ]           [         ]
'
' The main loop uses the I2C out command to start the PING on the SFR08,
' waits 70mS and then reads back from the HexEngine I2C port to gather the
' SFR08 echo. once it has the data, the light level is used to control the
' height of the ehxapod and the distance from the ranger is used to contol
' the y translate of the hexapod, if the SFR08 module is mounted on the
' front of the hexapdo this gives the effect of the hexapod moving away from
' an object in front of it, and cowering if a hand passes of the head.
'
' -----[ Revision History ]------------------------------------------------
'
' -----[ Compilation Switches ]--------------------------------------------

'#DEFINE FULL_DEBUG   ' COMPILE WITH PS2 DEBUG INFORMATION
'#DEFINE PIP_MODE_1   ' PIP MODE 1 FOR MORE ROBUST COMMS, 0 FOR SIMPLER COMMS
'
'
' -----[ I/O Definitions ]-------------------------------------------------
LED     PIN 0     ' CHANGE TO SUIT YOUR CONFIGURATION
SER_OUT PIN 5     ' CHANGE TO SUIT YOUR CONFIGURATION
SER_IN  PIN 6     ' CHANGE TO SUIT YOUR CONFIGURATION

' -----[ Constants ]-------------------------------------------------------

CMD_PIP_ESCAPE CON         $7d
CMD_PIP_HEADER CON         $7e
CMD_PIP_XOR    CON         $20

#SELECT $STAMP
  #CASE BS2, BS2E, BS2PE
    T1200       CON     813
    T2400       CON     396
    T4800       CON     188
    T9600       CON     84
    T19K2       CON     32
    T38K4       CON     6
  #CASE BS2SX, BS2P
    T1200       CON     2063
    T2400       CON     1021
    T4800       CON     500
    T9600       CON     240
    T19K2       CON     110
    T38K4       CON     45
    T57K6       CON     23
    T115K2      CON     2
  #CASE BS2PX
    T1200       CON     3313
    T2400       CON     1646
    T4800       CON     813
    T9600       CON     396
    T19K2       CON     188
    T38K4       CON     84
#ENDSELECT

COM_SevenBit        CON     $2000
COM_Inverted        CON     $4000
COM_Open            CON     $8000

Baud            CON     T19k2       ' match DEBUG


' -----[ Variables ]-------------------------------------------------------
PIP_Byte  VAR Byte
PIP_Len   VAR Byte
PIP_Temp  VAR Byte
PIP_Buff  VAR Byte(17)
PIP_Cs    VAR Byte
Range     VAR Byte
Light     VAR Byte
W_Temp    VAR Word
Temp      VAR Byte

' -----[ Initialization ]--------------------------------------------------
Setup:
HIGH SER_OUT


' -----[ Program Code ]----------------------------------------------------
Main:
  ' ALLOW THE HEXENGINE TO POWER UP
  PAUSE( 1500 )
  ' RESET LEGS IF ALREADY POWERD UP
  PIP_Buff(0) = "r"
  PIP_len = 1
  GOSUB Send_PIP_Packet

  DEBUG CLS

  DO
  DEBUG HOME
  ' WAKE HEXAPOD
  PIP_Buff(0) = "+"
  PIP_len = 1
  GOSUB Send_PIP_Packet

' -----[ I2C TEST Code ]----------------------------------------------------

' PINGS SRF08 MODULE CONNECTED TO I2C PORT, AND RETREIVES ECHO DATA.
' THIS ASSUMES THERE IS AN SRF08 MODULE CONNECTED, AND THAT IT HAS
' THE DEFAULT ADDRESS OF E0h
START_SFR08_PING:
  HIGH  LED
  ' SEND I2C DATA OUT OF I2C PORT
  PIP_Buff(0) = "I"
  PIP_Buff(1) = $e0   ' I2C ADDRESS
  PIP_Buff(2) = 1     ' I2C SEND DATA COUNT
  PIP_Buff(3) = 0     ' I2C REGISTER ADDRESS
  PIP_Buff(4) = 82    ' I2C DATA (PING.. RETURN DATA IN uSECONDS)
  PIP_len = 5
  GOSUB Send_PIP_Packet
  LOW LED

  ' WAIT FOR SFR08 ECHO MINIUMUM 65mS
  PAUSE(70)

GET_SRF08_ECHO:
  PIP_Buff(0) = "i"
  PIP_Buff(1) = $e0     ' I2C ADDRESS
  PIP_Buff(2) = 3       ' I2C DATA READ COUNT
  PIP_Buff(3) = 1       ' I2C REGISTER START ADDRESS
  PIP_len = 4
  GOSUB Send_PIP_Packet

  ' GRAB RETURN PACKET
  PIP_len = 4
  ' WAIT FOR HEADER, IGNORE PACKET LENGTH & CHECK SUM
  SERIN SER_IN, Baud , 100, NO_I2C_DATA, [WAIT($7E), PIP_Len, STR PIP_BuffPIP_Len]

  ' OUTPUT DATA TO DEBUG
  #IF FULL_DEBUG #THEN
  DEBUG "LEN: ", DEC2 PIP_Len, " TYPE: '" , PIP_Buff(0), "' "
  #ENDIF
  ' STORE RANGE RESULT IN WORD
  W_Temp = Pip_Buff(2) << 8
  W_Temp = W_Temp | PIP_Buff(3)
  Temp = PIP_Buff(1)
  #IF FULL_DEBUG #THEN
  DEBUG "LIGHT: ", DEC3 Temp, " RANGE: ", DEC4 W_Temp, " uS"
  #ENDIF

  ' REDUCE AND CLAMP RANGE RESULT
  W_Temp = W_Temp / 10
  IF W_Temp < 20 THEN W_Temp = 20
  IF W_Temp > 150 THEN W_Temp = 150

  ' INTERPOLATE RANGE RESULT
  Range = ( ( W_Temp - 20 ) * ( 255 ) ) / ( 150 - 20 )
  DEBUG CR, "RANGE: ", DEC3 Range

  ' CLAMP LIGHT RESULT
  IF Temp > 150 THEN Temp = 150

  ' INTERPOLATE RANGE RESULT
  Light = ( ( Temp ) * ( 255 ) ) / ( 150 )
  DEBUG CR, "LIGHT: ", DEC3 Light


SEND_BODY_CMD:
  PIP_Buff(0) = "B"
  PIP_Buff(1) = 0       '
  PIP_Buff(2) = 0       '
  PIP_Buff(3) = 0       '
  PIP_Buff(4) = 0       '
  PIP_Buff(5) = (255-Range) -128       '
  PIP_Buff(6) = Light -128       '
  PIP_len = 7
  GOSUB Send_PIP_Packet



  GOTO  CONTINUE

NO_I2C_DATA:
  DEBUG CLREOL, "NO I2C DATA"



CONTINUE:

  LOOP

  END



' -----[ PIP Packet ]----------------------------------------------------
' SENDS PIP PACKET STORED IN PIP_BUFF OF PIP_LEN LENGTH
' MODE 1 or MODE 0, DEPENDS ON COMPILATION SWITCH "PIP_MODE_1"

' SENDS PIP PACKET STORED IN PIP_BUFF OF PIP_LEN LENGTH
'
Send_PIP_Packet:
  ' SEND HEADER
  SEROUT SER_OUT, Baud, [CMD_PIP_HEADER]
  ' SEND PIP LENGHT
  PIP_Byte = PIP_Len
  GOSUB Send_Byte_Check_Codes
  ' SETUP CHECK SUM
  PIP_Cs = 0
  ' SEND PACKET
  FOR PIP_Temp = 0 TO PIP_Len-1
    PIP_Byte = PIP_Buff( PIP_Temp )
    PIP_Cs = PIP_Cs + PIP_Byte
    GOSUB Send_Byte_Check_Codes
  NEXT
  ' SEND CHECK SUM
  PIP_Byte = $ff - PIP_Cs
  GOSUB Send_Byte_Check_Codes

  RETURN


' SENDS PIP BYTE TO SERIAL PORT AND CHECKS PIP CODES
'
Send_Byte_Check_Codes:
#IF PIP_MODE_1 #THEN
  ' CHECK CODE AGAINST ILLEGAL CODES
  IF( PIP_Byte = CMD_PIP_HEADER | PIP_Byte = CMD_PIP_ESCAPE ) THEN
    ' SEND ESCAPED CODE
    SEROUT SER_OUT, Baud, [CMD_PIP_ESCAPE, PIP_Byte ^ CMD_PIP_XOR]
  ELSE
#ENDIF
    ' SEND NORMAL CODE
    SEROUT SER_OUT, Baud, [PIP_Byte]

#IF PIP_MODE_1 #THEN
  ENDIF
#ENDIF

  RETURN