' -----[ Title ]--------------------------------------------------------------
' Robotics with the Boe-Bot - FollowingBoeBot.bs2
' Boe-Bot adjusts its position to keep objects it detects in zone 2.

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

DEBUG "Program Running!"

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

Kpl            CON     -35
Kpr            CON     35
SetPoint       CON     2
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

' -----[ Subroutine – Get Pulse ]---------------------------------------------

Send_Pulse:
  PULSOUT 13,pulseLeft
  PULSOUT 12,pulseRight
  PAUSE 5
  RETURN