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

' what pins are being used, named as in the Roomba SCI
DD 		PIN 12
RXD		PIN 11
TXD		PIN 10
LED  		PIN  7
LSRB		PIN  2
LSRF		PIN  1
INPUT TXD

' baudmode 32 == 19200 8N1, not inverted
BAUDFAST	CON 32
' baudmode 396 == 2400 8N1, not inverted
BAUDSLOW	CON 396

' define some Roomba constants
R_START 	CON 128
R_BAUD	CON 129
R_CONTROL 	CON 130
R_FULL 	CON 132
R_POWER 	CON 133
R_SPOT 	CON 134
R_CLEAN 	CON 135
R_DRIVE	CON 137
R_MOTORS	CON 138
R_SONG	CON 140
R_PLAY	CON 141
R_SENSORS	CON 142

' sensor bytes
SENS_BUMPWHEEL	VAR BYTE
SENS_WALL 		VAR BYTE
SENS_CLIFF_L	VAR BYTE
SENS_CLIFF_FL	VAR BYTE
SENS_CLIFF_FR	VAR BYTE
SENS_CLIFF_R	VAR BYTE
SENS_VWALL		VAR BYTE
SENS_MOTOROVER	VAR BYTE
SENS_DIRT_L	VAR BYTE
SENS_DIRT_R	VAR BYTE
i VAR BYTE       ' counter
SA_POS VAR BYTE  ' servo A position -127 to 127
SB_POS VAR BYTE  ' servo B position -127 to 127
resF VAR WORD
resB VAR WORD
oldres VAR WORD

BUMP_RIGHT VAR SENS_BUMPWHEEL.BIT0
BUMP_LEFT  VAR SENS_BUMPWHEEL.BIT1

' wake up robot
LOW DD
PAUSE 100
HIGH DD
PAUSE 2000

' pulse device detect 3 times to shift down to 19.2kbps
FOR i = 1 TO 3
	LOW DD
     LOW LED
	PAUSE 250
	HIGH DD
     HIGH LED
	PAUSE 250
NEXT

SEROUT RXD, BAUDFAST, [R_START]
PAUSE 100
SEROUT RXD, BAUDFAST, [R_BAUD, 3] ' 3 == 2400 bps
PAUSE 100
SEROUT RXD, BAUDSLOW, [R_START]
PAUSE 100
SEROUT RXD, BAUDSLOW, [R_CONTROL]
PAUSE 100

result VAR WORD

Main:
     DEBUG "at the top",CR
	GOSUB Update_Sensors	

 	IF BUMP_RIGHT THEN
		GOSUB Spin_Left
		PAUSE 1000
	ELSEIF BUMP_LEFT THEN
		GOSUB Spin_Right
		PAUSE 1000
	ENDIF
	
	GOSUB Go_Forward
     PAUSE 100

	' check light levels, find the dark
	FOR i = 1 TO 4
	     oldres = resF
		GOSUB Read_Light
		IF resF > oldres THEN ' its getting darker
			GOSUB Go_Forward
		ELSE
			GOSUB Spin_Left
       		PAUSE 500
		ENDIF
	NEXT
		
	GOTO Main

Done:
	PAUSE 1000
	SEROUT RXD, BAUDSLOW, [R_POWER]  ' turn off
	STOP

Spin_Left:
	SEROUT RXD,BAUDSLOW, [R_DRIVE,$00,$c8,$00,$01]
	RETURN
Spin_Right:
	SEROUT RXD,BAUDSLOW, [R_DRIVE,$00,$c8,$ff,$ff]
	RETURN
Go_Forward:
	SEROUT RXD,BAUDSLOW, [R_DRIVE,$00,$c8,$80,$00]
	RETURN
Go_Stop:
	SEROUT RXD,BAUDSLOW, [R_DRIVE,$00,$00,$00,$00]
	RETURN

' Get sensor packet 1, which is 10 bytes
Update_Sensors:
	DEBUG "update sensors",CR
	SEROUT RXD, BAUDSLOW, [R_SENSORS, 1]
	SERIN  TXD, BAUDSLOW, 2000, No_Data, [SENS_BUMPWHEEL,SENS_WALL,
					SENS_CLIFF_L,SENS_CLIFF_FL,SENS_CLIFF_FR,SENS_CLIFF_R,
					SENS_VWALL,SENS_MOTOROVER,SENS_DIRT_L,SENS_DIRT_R]
	RETURN

No_Data:
     DEBUG "no data!",CR
	RETURN


READ_Light:
	HIGH LSRF  ' charge cap
	PAUSE 1    ' for 1 ms
	RCTIME LSRF, 1, resF
	DEBUG DEC ? resF
	RETURN