'------[ Title ]-----------------------------------------------------------------------
' For Demonstration - Bot2MoveingForwardWithLinesAndPingBottomBoard.bs2
' Date: 19 May 08
'
'------[ Notes ]-----------------------------------------------------------------------
'
'Used rechargeable 1.25V batteries.

'------[ Program ]---------------------------------------------------------------------

'{$STAMP BS2}                             'Stamp directive.
'{$PBASIC 2.5}                            'PBASIC directive.

'------[ Variables/Constants/Pins ]----------------------------------------------------


FreqSelect        VAR    Nib
IrFrequency       VAR    Word
IrDetectR         VAR    Bit
IrDetectL         VAR    Bit
DistanceR         VAR    Nib
DistanceL         VAR    Nib

PulseCount        VAR    Word

looping           VAR    Word
counter           VAR    Word

FreqDetectable    CON    3000


OutPin            PIN    15
InPin             PIN    14

Ping              PIN    0
Piezospeaker      PIN    4                'Speaker
WheelRight        PIN    12               'Wheels right/left.
WheelLeft         PIN    13

IrDetectLeft      PIN    9                'Left emitter/detector pair
IrLedLeft         PIN    8
IrDetectRight     PIN    0                'Right emitter/detector pair
IrLedRight        PIN    2

'------[ Initialization ]--------------------------------------------------------------

'FREQOUT Piezospeaker, 2000, FreqDetectable    'Signal program start/reset.

LOW OutPin

'------[ Main Routine ]----------------------------------------------------------------

looping = 0

PAUSE 18000       ' give time for top board to play song


DO 'Main loop

  DistanceR = 0
  DistanceL = 0

  FOR FreqSelect = 0 TO 2

    LOOKUP FreqSelect, [37500,39500,41500], IrFrequency
    FREQOUT IrLedRight, 1, IrFrequency
    IrDetectR = IN0
    FREQOUT IrLedLeft, 1, IrFrequency
    IrDetectL = IN9

    DistanceL = DistanceL + IrDetectL
    DistanceR = DistanceR + IrDetectR



  NEXT 'FreqSelect = 0 TO 4

  IF (DistanceL < 2) AND (DistanceR < 2) THEN

    looping = looping + 1

    PAUSE 500

    IF looping = 1 THEN
      GOSUB ForwardSmall

    ELSEIF looping = 2 THEN

      GOSUB Turn_right_pi

    ELSEIF looping = 3 THEN

      GOSUB Pivot_Right_pi_over_2           ' 1st turn into exchange zone
    ELSEIF looping = 4 THEN


      HIGH OutPin
      PAUSE 500
      LOW OutPin

      DO
        IF (IN14 = 1) THEN
          GOSUB Rest1
        ENDIF
        PAUSE 10
      LOOP

      Rest1:

      GOSUB Turn_left_pi                    ' 1st turn to exit exchange zone
    ELSEIF looping = 5 THEN
      GOSUB Pivot_Left_pi_over_2_but_less   ' 1st turn into Lego drop off zone
    ELSEIF looping = 6 THEN


      HIGH OutPin
      PAUSE 500
      LOW OutPin

      DO
        IF (IN14 = 1) THEN
          GOSUB Rest1b
        ENDIF
        PAUSE 10
      LOOP

      Rest1b:




      GOSUB BackSmall
      GOSUB Turn_right_pi                   ' 1st turn away from Lego drop off zone


    ELSEIF looping = 7 THEN
      GOSUB Pivot_Right_pi_over_2           ' 2nd turn into exchange zone
    ELSEIF looping = 8 THEN


      HIGH OutPin
      PAUSE 500
      LOW OutPin

      DO
        IF (IN14 = 1) THEN
          GOSUB Rest2
        ENDIF
        PAUSE 10
      LOOP

      Rest2:


      GOSUB Turn_left_pi                    ' 2nd turn to exit exchange zone
    ELSEIF looping = 9 THEN
      GOSUB Pivot_Left_pi_over_2_but_less   ' 2nd turn into Lego drop off zone
    ELSEIF looping = 10 THEN




      HIGH OutPin
      PAUSE 500
      LOW OutPin

      DO
        IF (IN14 = 1) THEN
          GOSUB Rest2b
        ENDIF
        PAUSE 10
      LOOP

      Rest2b:



      GOSUB BackSmall
      GOSUB Turn_right_pi                   ' 2nd turn away from Lego drop off zone


    ELSEIF looping = 11 THEN
      GOSUB Pivot_Right_pi_over_2           ' 3rd turn into exchange zone
    ELSEIF looping = 12 THEN


      HIGH OutPin
      PAUSE 500
      LOW OutPin

      DO
        IF (IN14 = 1) THEN
          GOSUB Rest3
        ENDIF
        PAUSE 10
      LOOP

      Rest3:



      GOSUB Turn_left_pi                    ' 3rd turn to exit exchange zone
    ELSEIF looping = 13 THEN
      GOSUB Pivot_Left_pi_over_2_but_less   ' 3rd turn into Lego drop off zone
    ELSEIF looping = 14 THEN



      HIGH OutPin
      PAUSE 500
      LOW OutPin

      DO
        IF (IN14 = 1) THEN
          GOSUB Rest3b
        ENDIF
        PAUSE 10
      LOOP

      Rest3b:


      GOSUB BackSmall
      GOSUB Turn_right_pi                   ' 3rd turn away from Lego drop off zone
    ELSEIF looping = 15 THEN
      END

    ENDIF ' looping = 1

  ENDIF ' (DistanceL < 2) AND (DistanceR < 2)



  PULSOUT WheelLeft, 751 + (DistanceL*6)      'move forward
  PULSOUT WheelRight, 750 - (DistanceR*6)


LOOP 'Main loop



'------[ Sub Routine ]----------------------------------------------------------------




BackSmall:

  FOR counter = 1 TO 25
    PULSOUT WheelLeft, 730
    PULSOUT WheelRight, 780
    PAUSE 20
  NEXT

RETURN


ForwardSmall:

  FOR counter = 1 TO 50
    PULSOUT WheelLeft, 780     ' Changed so that Bot 1 will go staight
    PULSOUT WheelRight, 720
    PAUSE 20
  NEXT

RETURN



Turn_left_pi:

  FOR PulseCount = 1 TO 100
    PULSOUT WheelLeft, 750 - 30
    PULSOUT WheelRight, 750 - 30
    PAUSE 20
  NEXT

RETURN



Turn_right_pi:

  FOR PulseCount = 1 TO 95
    PULSOUT WheelLeft, 750 + 30
    PULSOUT WheelRight, 750 + 30
    PAUSE 20
  NEXT


RETURN




Pivot_Left_pi_over_2_but_less:

' Ramp up left pivot

  FOR PulseCount = 1 TO 52
    PULSOUT WheelLeft, 750 - 2
    PULSOUT WheelRight, 750 - PulseCount
    PAUSE 20
  NEXT

' Ramp down left pivot

  FOR PulseCount = 52 TO 1
    PULSOUT WheelLeft, 750 - 2
    PULSOUT WheelRight, 750 - PulseCount
    PAUSE 20
  NEXT

RETURN




Pivot_Left_pi_over_2:

' Ramp up left pivot

  FOR PulseCount = 1 TO 62
    PULSOUT WheelLeft, 750
    PULSOUT WheelRight, 750 - PulseCount
    PAUSE 20
  NEXT

' Ramp down left pivot

  FOR PulseCount = 62 TO 1
    PULSOUT WheelLeft, 750
    PULSOUT WheelRight, 750 - PulseCount
    PAUSE 20
  NEXT

RETURN



Pivot_Right_pi_over_2:

  FOR PulseCount = 1 TO 60
    PULSOUT WheelLeft, 750 + 30
    PULSOUT WheelRight, 750 + 2
    PAUSE 20
  NEXT


RETURN