' BOEGripperIF.bs2   SIMPLE BOE-Bot Gripper Test Using Infra Red Detection by Nishi

' {$STAMP BS2}
' {$PBASIC 2.5}

DEBUG "Program Running!"



irDetectLeft VAR Bit                                            ' Variable Declarations..
irDetectRight VAR Bit
pulseLeft VAR Word
pulseRight VAR Word
pulseGrip   VAR   Word

DEBUG "Testing piezospeaker..."
FREQOUT 4, 2000, 3000                                           ' Signal program start/reset..

DEBUG CLS,
  "IR DETECTORS", CR,
  "Left Right", CR,
  "----- -----"

DO                                                              ' Main Routine..

    FREQOUT 8, 1, 38500                                         ' Check IR Detectors..
    irDetectLeft = IN9

    FREQOUT 2, 1, 38500
    irDetectRight = IN0
                                                                ' Gripper Action..
          IF (irDetectLeft = 0) AND (irDetectRight = 0) THEN    'Gripper IF..
           HIGH 10
           HIGH 1

          pulseGrip = 1000                                           ' Close Gripper


          ELSE
          LOW 10
          LOW 1
          pulseGrip = 500                                            ' Open Gripper

          ENDIF


          DEBUG CRSRXY, 2, 3, BIN1 irDetectLeft,
        CRSRXY, 9, 3, BIN1 irDetectRight

          PAUSE 20                                                  '(we PAUSE otherwise the servo hums,
                                                                     'gotta give the program time to run.)

        PULSOUT 13,pulseLeft                                    ' Apply the pulse..
        PULSOUT 12,pulseRight
        PULSOUT 14,pulseGrip
LOOP 