' -----[ Title ]--------------------------------------------------------------
' Robotics with the Boe-Bot - StripeFollowingBoeBot.bs2
' Boe-Bot adjusts its position to move toward objects that are closer than
' zone 3 and away from objects further than zone 3.  Useful for following a
' 2.25 inch wide vinyl electrical tape stripe.
' {$STAMP BS2}                               ' Stamp directive.
' {$PBASIC 2.5}                              ' PBASIC directive.

DEBUG "Program Running!"

' -----[ Constants ]----------------------------------------------------------

Kpl            CON     35                    ' Change from -35 to 35
Kpr            CON     -35                   ' Change from 35 to -35
SetPoint       CON     3                     ' Change from 2 to 3.
CenterPulse    CON     750
' -----[ Variables ]----------------------------------------------------------

freqSelect     VAR     Nib
irFrequency    VAR     Word
irDetectLeft   VAR     Bit
irDetectRight  VAR     Bit
distanceLeft   VAR     Nib
distanceRight  VAR     Nib
pulseLeft      VAR     Word
pulseRight     VAR     Word

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

FREQOUT 4, 2000, 3000

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

DO

  GOSUB Get_Ir_Distances

  ' Calculate proportional output.

  pulseLeft =  SetPoint - distanceLeft  * Kpl + CenterPulse
  pulseRight = SetPoint - distanceRight * Kpr + CenterPulse

  GOSUB Send_Pulse

LOOP

' -----[ Subroutine - Get IR Distances ]--------------------------------------

Get_Ir_Distances:
  distanceLeft = 0
  distanceRight = 0
  FOR freqSelect = 0 TO 4
    LOOKUP freqSelect,[37500,38250,39500,40500,41500], irFrequency

    FREQOUT 8,1,irFrequency
    irDetectLeft = IN9
    distanceLeft = distanceLeft + irDetectLeft

    FREQOUT 2,1,irFrequency
    irDetectRight = IN0
    distanceRight = distanceRight + irDetectRight
  NEXT
  RETURN