'{$STAMP BS2}
'------------------------------------------
'golight.bs2
'
'This program scans the light sensor using the head
'motor and moves the robot in the direction of the
'light.

lightl  var word           'Light Left
lightc  var word           'Light Center
lightr  var word           'Light Right

net     con 8              'Coprocessor network pin
baud    con 396            'Coprocessor baud rate

        low 9                           'Speaker off
        serout net,baud,["!1R180"]      'Center steering
        pause 100
here:   if in14=1 then here             'Wait for button


start:  if in0=1 and in1=1 then findi   'Stop if whisker
        serout net,baud,["!1M1100000"]  'Stop
        pause 100
        goto start                      'Wait for no whisker

findi:  serout net,baud,["!1M116FFFF"]  'Drive motor forward
        pause 100

        gosub getls                     'Get light level
        debug dec lightl,"-",dec lightc,"-",dec lightr,cr

        'Left brightest? (lowest value)
        if lightl < lightc and lightl < lightr then findl

        'Right brightest? (lowest value)
        if lightr < lightc and lightr < lightl then findr

        'Center must be brightest.
        serout net,baud,["!1R180"]      'Center steering
        pause 100
        goto start

findl:  serout net,baud,["!1R1D0"]      'Steer left.
        pause 100
        goto start

findr:  serout net,baud,["!1R120"]      'Steer right.
        pause 100
        goto start



'------------------------------------------
'getls 
'Scans and Reads the light levels at left, center, right.

getls:
        serout net,baud,["!1R2FF"]  'Move head right
        pause 300
        high 4                      'Discharge cap
        pause 1                     'Short delay
        rctime 4,1,lightr           'Capture light level

        serout net,baud,["!1R201"]  'Move head left
        pause 600
        high 4                      'Discharge cap
        pause 1                     'Short delay
        rctime 4,1,lightl           'Capture light level

        serout net,baud,["!1R280"]  'Move head center
        pause 300
        high 4                      'Discharge cap
        pause 1                     'Short delay
        rctime 4,1,lightc           'Capture light level

        serout net,baud,["!1R200"]  'Head motor off
        pause 100

        return                      'Done