' =========================================================================
'   File...... Fighting Robots.bsp
'   Purpose... Prototype This: Fighting Robots, Sensor/Health Control Board
'   Author.... J. Grand
'
' {$STAMP BS2}
'   {$PBASIC 2.5}

' -----[ Program Description ]---------------------------------------------

' This program takes in RS232 serial data from a PC.
' It provides the status of the four Hall Effect sensors mounted on the
' robots (one on head, one on body for each robot) and also controls the
' output of the Health-O-Meter. 
'
' NOTE: RC-4 code will hang if we try to address a module that does
' not exist. 
'
'   4 byte input: <header>, <command>, <value>
'   1 byte response: <ack> or <status of sensors> depending on command
'
'   Set Score: "!", 0, aa, bb  - Robot 1 (high nibble), Robot 2 (low nibble)
'                              - aa: health: %0000-%1001 (0-9)
'                              - bb: %0000 off, %0001 to trigger money-shot/winner light sequence 
'   Response: $AA			- Ack

'   Get Sensors: "!", 1, yy, yy   - yy: Bytes ignored
'   Response: zz			   - zz bits: 0, 0, 0, 0, Robot1_Head, Robot1_Body, Robot2_Head, Robot2_Body
' 
'   [see source code for more details]
'

' -----[ I/O Definitions ]-------------------------------------------------

PC_RX			PIN	   	13 	  		' Serial input FROM PC (TTL-level)
PC_TX     		PIN 		12			' Serial output TO PC

RC4_Data0       PIN       8
RC4_Data1       PIN       7

' Hall Effect sensor inputs (active low, HIGH = no detection)
Robot1_Head   	PIN    	0 
Robot1_Body	PIN		1
Robot2_Head	PIN		2
Robot2_Body	PIN		3

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

#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
#ENDSELECT

SevenBit    CON     	$2000
Inverted    CON     	$4000
Open        CON     	$8000

PC_Baud	  CON		T9600
RC4_Baud    CON     Open + T38K4

IsOn            CON     1
IsOff           CON     0


' -----[ Variables ]-------------------------------------------------------

cmd		VAR	BYTE
var1		VAR	BYTE
var2 		VAR	BYTE
sensors	VAR	BYTE

' RC-4
Addr       VAR	 BYTE	' %00 - %11

id         VAR  BYTE(3)
'status     VAR  BYTE

' -----[ Initialization ]--------------------------------------------------

  DEBUG CLS, "Fighting Robots Health-O-Meter/Sensor Control Board!", CR

Init:
   'INPUT robot1_head
   'INPUT robot1_body
   'INPUT robot2_head
   'INPUT robot2_body

  ' RC-4 module initialization
  FOR Addr = %00 TO %11
    SEROUT RC4_Data0, RC4_Baud, ["!RC4", Addr, "V"]         ' get version
    SERIN  RC4_Data0, RC4_Baud, 200, Init, [STR id\3]
'    DEBUG CR, "Data0: Addr ", BIN2 Addr, " RC-4 Version ", STR id\3
  NEXT

  FOR Addr = %00 TO %00
    SEROUT RC4_Data1, RC4_Baud, ["!RC4", Addr, "V"]  
    SERIN  RC4_Data1, RC4_Baud, 200, Init, [STR id\3]
'    DEBUG CR, "Data1: Addr ", BIN2 Addr, " RC-4 Version ", STR id\3
  NEXT

  FOR Addr = %00 TO %11
    SEROUT RC4_Data0, RC4_Baud, ["!RC4", Addr, "X"]        ' all off
'    GOSUB Show_Status0
  NEXT

  FOR Addr = %00 TO %00
    SEROUT RC4_Data1, RC4_Baud, ["!RC4", Addr, "X"]   
'    GOSUB Show_Status1
  NEXT

  DEBUG CR, "RDY!", CR

' -----[ Program Code ]----------------------------------------------------

Main:
  SERIN PC_RX, PC_Baud, [WAIT("!"), cmd, var1, var2]
  DEBUG CR, "C:", cmd, " V1:", HEX var1, " V2:", HEX var2, CR

  IF (cmd = $30) THEN
    DEBUG "Health: R1=", HEX var1.HIGHNIB, " R2=", HEX var1.LOWNIB, CR
    DEBUG "Winner: R1=%", BIN4 var2.HIGHNIB, " R2=%", BIN4 var2.LOWNIB, CR

    GOSUB Robot1_Relays
    GOSUB Robot2_Relays

    SEROUT PC_TX, PC_Baud, [$AA]	 ' send ACK back to Zoz

    ' trigger the money-shot/winner light sequence if the game is over
    IF (var2.HIGHNIB = 1) THEN ' robot 1 wins
      'SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 3, IsOn]
      var1.LOWNIB = 0 ' make sure we disable the other robot's health-o-meter
      GOSUB Robot2_Relays

     DO WHILE (var1.HIGHNIB <= 9) ' bring winning robot's score back up to max
       GOSUB Robot1_Relays
       PAUSE 250
       var1.HIGHNIB = var1.HIGHNIB + 1
     LOOP
 
    FOR sensors = 0 TO 4
      FOR cmd = 0 TO 3 ' blink the health-o-meter
         var1.HIGHNIB = 0
         GOSUB Robot1_Relays
         PAUSE 75
         var1.HIGHNIB = 9
         GOSUB Robot1_Relays
         PAUSE 75
      NEXT

      FOR cmd = 0 TO 3 ' increase and decrease the health-o-meter
        FOR var1.HIGHNIB = 0 TO 9
           GOSUB Robot1_Relays
           PAUSE 25
        NEXT
        FOR var1.HIGHNIB = 9 TO 0
           GOSUB Robot1_Relays
           PAUSE 25
        NEXT
      NEXT 
    NEXT

      GOTO Init ' game is over, so reset the system and start again!

    ENDIF

    IF (var2.LOWNIB = 1) THEN ' robot 2 wins
      'SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 4, IsOn]
      var1.HIGHNIB = 0 ' make sure we disable the other robot's health-o-meter
      GOSUB Robot1_Relays

      DO WHILE (var1.LOWNIB <= 9) ' bring winning robot's score back up to max
          GOSUB Robot2_Relays
          PAUSE 250
          var1.LOWNIB = var1.LOWNIB + 1
      LOOP
 
  FOR sensors = 0 TO 4
      FOR cmd = 0 TO 3 ' blink the health-o-meter
         var1.LOWNIB = 0
         GOSUB Robot2_Relays
         PAUSE 75
         var1.LOWNIB = 9
         GOSUB Robot2_Relays
         PAUSE 75
      NEXT

      FOR cmd = 0 TO 3 ' increase and decrease the health-o-meter
        FOR var1.LOWNIB = 0 TO 9
           GOSUB Robot2_Relays
           PAUSE 25
        NEXT
        FOR var1.LOWNIB = 9 TO 0
           GOSUB Robot2_Relays
           PAUSE 25
        NEXT
      NEXT 
  NEXT

      GOTO Init ' game is over, so reset the system and start again!
    ENDIF

  ELSEIF (cmd = $31) THEN
	' get status of all hall effect sensors
	sensors = 0
	IF (Robot1_Head = 0) THEN sensors = sensors | 1
	sensors = sensors << 1
	IF (Robot1_Body = 0) THEN sensors = sensors | 1
	sensors = sensors << 1
	IF (Robot2_Head = 0) THEN sensors = sensors | 1
	sensors = sensors << 1
	IF (Robot2_Body = 0) THEN sensors = sensors | 1

     DEBUG BIN4 sensors, CR
	SEROUT PC_TX, PC_Baud, [sensors]	 ' send status of sensors back to Zoz
  ELSE
  	'DEBUG "Bad cmd!", CR  
  ENDIF

  GOTO Main

END

' -----[ Subroutines ]-----------------------------------------------------

'Show_Status0:
'  SEROUT RC4_Data0, RC4_Baud, ["!RC4", Addr, "G"]                 ' get status
'  SERIN  RC4_Data0, RC4_Baud, 200, Show_Status0, [status]
'  DEBUG CR, "Data0: Addr ", BIN2 Addr, " Status = ", BIN4 status
'  RETURN

'Show_Status1:
'  SEROUT RC4_Data1, RC4_Baud, ["!RC4", Addr, "G"]                 ' get status
'  SERIN  RC4_Data1, RC4_Baud, 200, Show_Status1, [status]
'  DEBUG CR, "Data1: Addr ", BIN2 Addr, " Status = ", BIN4 status
'  RETURN

Robot1_Relays:
    ' enable relays for robot 1 depending on the score
    SELECT (var1.HIGHNIB)
      CASE 9
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 0, "S", %1111]       ' set outputs
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 1, "S", %1111] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 1, IsOn]
      CASE 8
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 0, "S", %1110]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 1, "S", %1111] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 1, IsOn]
      CASE 7
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 0, "S", %1100]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 1, "S", %1111] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 1, IsOn]
      CASE 6
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 0, "S", %1000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 1, "S", %1111] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 1, IsOn]
      CASE 5
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 0, "S", %0000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 1, "S", %1111] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 1, IsOn]
      CASE 4
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 0, "S", %0000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 1, "S", %1110] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 1, IsOn]
      CASE 3
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 0, "S", %0000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 1, "S", %1100] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 1, IsOn]
      CASE 2
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 0, "S", %0000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 1, "S", %1000] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 1, IsOn]
      CASE 1
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 0, "S", %0000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 1, "S", %0000] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 1, IsOn]
      CASE 0
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 0, "S", %0000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 1, "S", %0000] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 1, IsOff]
    ENDSELECT
    RETURN

Robot2_Relays:
   ' enable relays for robot 2 depending on the score
    SELECT (var1.LOWNIB)
      CASE 9
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 2, "S", %1111]       ' set outputs
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 3, "S", %1111] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 2, IsOn]
      CASE 8
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 2, "S", %1110]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 3, "S", %1111] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 2, IsOn]
      CASE 7
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 2, "S", %1100]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 3, "S", %1111] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 2, IsOn]
      CASE 6
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 2, "S", %1000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 3, "S", %1111] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 2, IsOn]
      CASE 5
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 2, "S", %0000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 3, "S", %1111] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 2, IsOn]
      CASE 4
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 2, "S", %0000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 3, "S", %1110] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 2, IsOn]
      CASE 3
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 2, "S", %0000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 3, "S", %1100] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 2, IsOn]
      CASE 2
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 2, "S", %0000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 3, "S", %1000] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 2, IsOn]
      CASE 1
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 2, "S", %0000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 3, "S", %0000] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 2, IsOn]
      CASE 0
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 2, "S", %0000]
        SEROUT RC4_Data0, RC4_Baud, ["!RC4", 3, "S", %0000] 
        SEROUT RC4_Data1, RC4_Baud, ["!RC4", 0, "R", 2, IsOff]
    ENDSELECT
    RETURN