'------[ Title ]-----------------------------------------------------------------------
' For Demonstration - Bot1IrMoevementsWthComunicationBottomBoard.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


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.


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

looping = 0

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                      ' 1st toward Lego pick up


    ELSEIF looping = 2 THEN
      PAUSE 12000
      GOSUB BackSmall
      GOSUB Turn_right_pi                     ' 1st away from Lego pick up
    ELSEIF looping = 3 THEN
      GOSUB Pivot_Left_pi_over_2              ' 1st toward Lego exchange zone
    ELSEIF looping = 4 THEN
      PAUSE 1000
      GOSUB BackSmall
      GOSUB Turn_right_pi                     ' 1st away from Lego exchange zone
    ELSEIF looping = 5 THEN
      GOSUB Pivot_right_pi_over_2             ' 2nd toward Lego pick up


    ELSEIF looping = 6 THEN
      PAUSE 12000
      GOSUB BackSmall
      GOSUB Turn_right_pi                     ' 2nd away from Lego pick up
    ELSEIF looping = 7 THEN
      GOSUB Pivot_Left_pi_over_2              ' 2nd toward Lego exchange zone
    ELSEIF looping = 8 THEN
      PAUSE 1000
      GOSUB BackSmall
      GOSUB Turn_right_pi                     ' 2nd away from Lego exchange zone
    ELSEIF looping = 9 THEN
      GOSUB Pivot_right_pi_over_2             ' 3rd toward Lego pick up


    ELSEIF looping = 10 THEN
      PAUSE 12000
      GOSUB BackSmall
      GOSUB Turn_right_pi                     ' 3rd away from Lego pick up
    ELSEIF looping = 11 THEN
      GOSUB Pivot_Left_pi_over_2              ' 3rd toward Lego exchange zone
    ELSEIF looping = 12 THEN
      PAUSE 1000
      GOSUB BackSmall
      GOSUB Turn_right_pi                     ' 3rd away from Lego exchange zone
    ELSEIF looping = 13 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_right_pi:                    ' Ramping needed to maintain uniform motion

' Ramp up right rotate

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

' Ramp down right rotate

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

RETURN




Pivot_Left_pi_over_2:

' Ramp up left pivot

  FOR PulseCount = 1 TO 52
    PULSOUT WheelLeft, 750 + (PulseCount/8)
    PULSOUT WheelRight, 750 - PulseCount
    PAUSE 20
  NEXT

' Ramp down left pivot

  FOR PulseCount = 52 TO 1
    PULSOUT WheelLeft, 750 + (PulseCount/8)
    PULSOUT WheelRight, 750 - PulseCount
    PAUSE 20
  NEXT

RETURN



Pivot_right_pi_over_2:

' Ramp up right pivot

  FOR PulseCount = 1 TO 56
    PULSOUT WheelLeft, 750 + PulseCount
    PULSOUT WheelRight, 750 - (PulseCount/4)
    PAUSE 20
  NEXT

' Ramp down right pivot

  FOR PulseCount = 56 TO 1
    PULSOUT WheelLeft, 750 + PulseCount
    PULSOUT WheelRight, 750 - (PulseCount/4)
    PAUSE 20
  NEXT

RETURN