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

' BasicStamp 2 program for Sumobot interface to Android phone using
' CDS cells. Written by Eric Gregori ( www.EMGRobotics.com )

'------------------------------------------------------------------------------------------
' CONSTANTS
'------------------------------------------------------------------------------------------
SPEED       CON          25
SSTOP       CON          744
LEFTFORW    CON          SSTOP+SPEED
LEFTBACK    CON          SSTOP-SPEED
RIGHTFORW   CON          SSTOP-SPEED
RIGHTBACK   CON          SSTOP+SPEED

'------------------------------------------------------------------------------------------
' Pin assignments
'------------------------------------------------------------------------------------------
LEFTCDS          PIN     0
RIGHTCDS         PIN     1
RIGHTSERVOS      PIN     13
LEFTSERVOS       PIN     12

'------------------------------------------------------------------------------------------
' Variables
'------------------------------------------------------------------------------------------
threshold       VAR      Byte


threshold = 2
autocalibrate:
  HIGH  LEFTCDS
  HIGH  RIGHTCDS
  PAUSE 1
  INPUT LEFTCDS
  INPUT RIGHTCDS
  PAUSE threshold
  IF LEFTCDS = 1 AND RIGHTCDS = 1 THEN GOTO servoloop
  threshold = threshold + 1
  IF threshold > 18 THEN GOTO error
  GOTO autocalibrate

servoloop:
  HIGH  LEFTCDS
  HIGH  RIGHTCDS
  PAUSE 1
  INPUT LEFTCDS
  INPUT RIGHTCDS
  PAUSE threshold
  IF LEFTCDS = 0 THEN
    PULSOUT LEFTSERVOS,  LEFTFORW
  ELSE
    PULSOUT LEFTSERVOS,  SSTOP
  ENDIF
  IF RIGHTCDS = 0 THEN
    PULSOUT RIGHTSERVOS,  RIGHTFORW
  ELSE
    PULSOUT RIGHTSERVOS,  SSTOP
  ENDIF
  PAUSE 18-threshold
  GOTO servoloop

error:
    PAUSE 19
    PULSOUT LEFTSERVOS,  LEFTFORW
    PULSOUT RIGHTSERVOS,  RIGHTBACK
    GOTO error