' {$STAMP BS2}
' pin 0 = Device detect
' pin 1 = RXD (INPUT TO roomba)
' pin 2 = TXD (OUTPUT from roomba)
' pin 3 = input to 4066 for cliff sens control (low/open normal- high high sens

'begin full control mode
'move forward straight (TBD) to ensure line is stradled

'drive straight forward beotch
'monitor cliff sensors gosub appropriate turn sub when activated
'monitor bump sensors gosub turn 180 when activated
'gosub drive straight forward beotch

x       VAR      Byte
TRASH   VAR      Byte
BUTT    VAR      Byte
BUMP    VAR      Byte
LLINE   VAR      Byte
RLINE   VAR      Byte


RLINE = 0
LLINE = 0

MAIN:

    GOSUB SETBAUD  'sets roomba baud to 19200 via dd line
    GOSUB INITROO  'sets roomba baud to 2400 via serial command, sets roomba to passive mode
    GOSUB LOOKBUTT 'monitors remote control input, breaks sub when right circle is pressed
    GOSUB PLAYSONG 'sets roomba to safe control mode, plays tune to indicate begenning of line following routine
    GOSUB CLEANGO  'starts roomba normal clean operation, sets cliff sensoes to high sens
    GOSUB CLEANLOK 'monitors cliffsensors while roomba cleans normally, exits on line/cliff detect
    GOSUB LINEHIT  '
    GOSUB SENSE

END


SETBAUD:
'pulls device detect pin low 3 times on order to set roomba baud to 19200

    PAUSE 1000
    FOR x = 1 TO 3
    LOW 0
    PAUSE 200
    INPUT 0
    PAUSE 200
    NEXT

RETURN


INITROO:

    SEROUT 1, 32, [129,3] 'sets roombaud to 2400
    PAUSE 100
    SEROUT 1, 396, [128] 'sets roomba to passive mode
    PAUSE 100

RETURN


LOOKBUTT:

    SEROUT 1, 396, [142,2] 'requests sensor data packet 2
    SERIN 2, 396, [BUTT]
    PAUSE 100
    IF BUTT <> 131 THEN LOOKBUTT 'exits sub if right circle remote button is pushed
    PAUSE 100

RETURN


PLAYSONG:

    SEROUT 1, 396, [130]
    PAUSE 100
    SEROUT 1, 396, [140,0,2,95,64,51,31] 'defines song
    PAUSE 100
    SEROUT 1, 396, [141,0] 'plays song
    PAUSE 2000

RETURN


CLEANGO:

SEROUT 1, 396, [135] 'sets roomba to normal clean mode
HIGH 3 ' sets cliff sensors to high sens mode

RETURN



CLEANLOK:

    PAUSE 10
    SEROUT 1, 396, [142,1] 'requests sensor data packet 1
    SERIN 2, 396, [BUMP]
    SERIN 2, 396, [TRASH]
    SERIN 2, 396, [LLINE]
    SERIN 2, 396, [TRASH]
    SERIN 2, 396, [TRASH]
    SERIN 2, 396, [RLINE]
    IF RLINE < 1 AND LLINE < 1 THEN CLEANLOK
    PAUSE 300

RETURN


LINEHIT:

    SEROUT 1, 396, [128]
    PAUSE 100
    SEROUT 1, 396, [130]
    PAUSE 100
    SEROUT 1, 396, [132]
    PAUSE 100
    SEROUT 1, 396, [140,1,2,117,64,75,64] 'defines song
    PAUSE 100
    SEROUT 1, 396, [141,1] 'plays song
    PAUSE 1500
    SEROUT 1, 396, [137, 01, 94, 128, 00]
    PAUSE 200

RETURN


SENSE:

    GOSUB STRAIGHT
    BUMP = 0
    TRASH = 0
    LLINE = 0
    RLINE = 0
    SEROUT 1, 396, [142,1] 'requests sensor data packet 1
    SERIN 2, 396, [BUMP]
    SERIN 2, 396, [TRASH]
    SERIN 2, 396, [LLINE]
    SERIN 2, 396, [TRASH]
    SERIN 2, 396, [TRASH]
    SERIN 2, 396, [RLINE]
    IF RLINE > 0 THEN RTURN
    IF LLINE > 0 THEN LTURN
    IF BUMP > 0 THEN UTURN
    GOSUB SENSE

RETURN


STRAIGHT:

    SEROUT 1, 396, [137, 01, 94, 128, 00]

RETURN


LTURN:

    SEROUT 1, 396, [137, 00, 200, 0, 1]
    PAUSE 500
    GOSUB SENSE

RETURN


RTURN:

    SEROUT 1, 396, [137, 00, 200, 255, 255]
    PAUSE 500
    GOSUB SENSE

RETURN


UTURN:

    SEROUT 1, 396, [137, 00, 64, 0, 1]
    PAUSE 9000
    GOSUB SENSE

RETURN