' -----[ Title ]--------------------------------------------------------------
' Robotics with the Boe-Bot - EscapingCorners.bs2
' Boe-Bot navigates out of corners by detecting alternating whisker presses.
' {$STAMP BS2}                               ' Stamp directive.
' {$PBASIC 2.5}                              ' PBASIC directive.

DEBUG "Program Running!"

' -----[ Variables ]----------------------------------------------------------

pulseCount     VAR     Byte                  ' FOR...NEXT loop counter.
counter        VAR     Nib                   ' Counts alternate contacts.
old7           VAR     Bit                   ' Stores previous IN7.
old5           VAR     Bit                   ' Stores previous IN5.

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

FREQOUT 4, 2000, 3000                        ' Signal program start/reset.
counter = 1                                  ' Start alternate corner count.
old7 = 0                                     ' Make up old values.
old5 = 1

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

DO

' ---  Detect Consecutive Alternate Corners ------------------------
' See the "How EscapingCorners.bs2 Works" section that follows this program.

  IF (IN7 <> IN5) THEN                       ' One or other is pressed.
    IF (old7 <> IN7) AND (old5 <> IN5) THEN  ' Different from previous.
      counter = counter + 1                  ' Alternate whisker count + 1.
      old7 = IN7                             ' Record this whisker press
      old5 = IN5                             ' for next comparison.
      IF (counter > 4) THEN                  ' If alternate whisker count = 4,
        counter = 1                          ' reset whisker counter
        GOSUB Back_Up                        ' and execute a U-turn.
        GOSUB Turn_Left
        GOSUB Turn_Left
      ENDIF                                  ' ENDIF counter > 4.
    ELSE                                     ' ELSE (old7=IN7) or (old5=IN5),
      counter = 1                            ' not alternate, reset counter.
    ENDIF                                    ' ENDIF (old7<>IN7) and
                                             ' (old5<>IN5).
  ENDIF                                      ' ENDIF (IN7<>IN5).



' ---  Same navigation routine from RoamingWithWhiskers.bs2 ------------------

  IF (IN5  = 0) AND (IN7 = 0) THEN           ' Both whiskers detect obstacle
    GOSUB Back_Up                            ' Back up & U-turn (left twice)
    GOSUB Turn_Left
    GOSUB Turn_Left
  ELSEIF (IN5  = 0) THEN                     ' Left whisker contacts
    GOSUB Back_Up                            ' Back up & turn right
    GOSUB Turn_Right
  ELSEIF (IN7  = 0) THEN                     ' Right whisker contacts
    GOSUB Back_Up                            ' Back up & turn left
    GOSUB Turn_Left
  ELSE                                       ' Both whiskers 1, no contacts
    GOSUB Forward_Pulse                      ' Apply a forward pulse
  ENDIF                                      ' and check again

LOOP

' -----[ Subroutines ]--------------------------------------------------------

Forward_Pulse:                               ' Send a single forward pulse.
  PULSOUT 13,850
  PULSOUT 12,650
  PAUSE 20
  RETURN

Turn_Left:                                   ' Left turn, about 90-degrees.
  FOR pulseCount = 0 TO 20
    PULSOUT 13, 650
    PULSOUT 12, 650
    PAUSE 20
  NEXT
  RETURN

Turn_Right:
  FOR pulseCount = 0 TO 20                   ' Right turn, about 90-degrees.
    PULSOUT 13, 850
    PULSOUT 12, 850
    PAUSE 20
  NEXT
  RETURN

Back_Up:                                     ' Back up.
  FOR pulseCount = 0 TO 40
    PULSOUT 13, 650
    PULSOUT 12, 850
    PAUSE 20
  NEXT
  RETURN