'Robotics with the Boe-Bot - SumoBoeBot.bs2
'Search for object, lock onto it and push it

'{$STAMP BS2}
' {$PBASIC 2.5}



irDetectLeft        VAR  Bit  'Left IR reading
irDetectRight       VAR  Bit  'Right IR reading
pulseLeft           VAR  Word 'pulse values for servos
pulseRight          VAR  Word
pulseGrip           VAR  Word
'[Initialization]

DEBUG "Program Running!"
FREQOUT 4, 2000, 3000

DEBUG CLS,
  "IR DETECTORS", CR,
  "LEFT RIGHT", CR
'[Main Routine]

Main:

'Spin around slowly until an object is spotted
  DO
    PULSOUT 13, 790       'Rotate slowly
    PULSOUT 12, 790
    PAUSE 15              '5ms for detectors
    GOSUB Check_IRs       'While looking for object

  LOOP UNTIL (irDetectLeft = 0) OR (irDetectRight = 0)

  'Now figure out exactly where the object is ANd go toward it
  DO
   FREQOUT 8, 1, 38500
   irDetectLeft = IN0
   FREQOUT 2, 1, 38500
   irDetectRight = IN9
  'Object in both detectors -- go forward
    IF (irDetectLeft = 0) AND (irDetectRight = 0) THEN
      HIGH 10
      HIGH 1
      pulseLeft = 850     'Forward
      pulseRight = 650
      pulseGrip = 500


  'Object on Left - go left
    ELSEIF (irDetectLeft = 0) THEN
      pulseLeft = 850    'Left toward object
      pulseRight = 850
  'Object on right - go right
    ELSEIF (irDetectRight = 0) THEN
      pulseLeft = 650     'Right toward object
      pulseRight = 650
'No object -- go forward anyway, because the detectors will
  ELSE                   'momentarily
    LOW 10
    LOW 1
    pulseLeft = 850      '"no object" as the
    pulseRight = 650     'Boe-Bot is adjusting
    pulseGrip = 1000
ENDIF

  PULSOUT 13, pulseLeft
  PULSOUT 12, pulseRight
  PULSOUT 14, pulseGrip
  PAUSE 15

'Check IRs again in case object is moving
  GOSUB Check_IRs
  LOOP

'[Subroutines]

Check_IRs:
  FREQOUT 8, 1, 38500
  irDetectLeft = IN0
  FREQOUT 2, 1, 38500
  IrDetectRight = IN9
  RETURN 