BOB_subs

' {$STAMP BS2}
' {$PBASIC 2.5}
'Sample Sonar test....
DEBUG "Mobility Functions!!",CR
PAUSE 500

GOSUB Jingle                  'Start Program Tone

counter         VAR     Word        'Required for functions
counter1  VAR Word
lcounter      VAR    Bit        'Required for functions
rawDist         VAR     Word
cm              VAR     Word
init_far CON 2000
init_large CON 4000





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

'-----[MAIN]-------------------------------------------------
cm =1000
rawDist = 2501
DO
  counter1 = 0
  lcounter = 0
  DO UNTIL (lcounter = 1)
    GOSUB Look_Straight

    FOR counter = 1 TO 24
        PULSOUT 15, 1500
        GOSUB Get_Sonar
        IF rawDist <init_far + counter1 THEN
          PAUSE 20
            GOSUB Look_Straight
            rawDist = init_large
          DO  UNTIL rawDist <init_far + counter1

            GOSUB Get_Sonar
            'DEBUG CLS,DEC rawDist
            GOSUB Left_Pulse
            PAUSE 21
          LOOP

          PAUSE 20

          DO  WHILE rawDist <init_far + counter1
                  GOSUB Get_Sonar
                  'DEBUG CLS,DEC rawDist
                  GOSUB Forward_Pulse
                  PAUSE 21
          LOOP
        EXIT
        ENDIF
    NEXT

    PAUSE 20
    GOSUB Look_Straight
    PAUSE 60

    FOR counter = 1 TO 24
        PULSOUT 15, 1
        GOSUB Get_Sonar
        IF rawDist <init_far + counter1 THEN
          PAUSE 20
            GOSUB Look_Straight
            rawDist = init_large
          DO  UNTIL rawDist <init_far + counter1

            GOSUB Get_Sonar
            'DEBUG CLS,DEC rawDist
            GOSUB Right_Pulse
            PAUSE 21
          LOOP

          PAUSE 20

          DO  WHILE rawDist <init_far + counter1
                  GOSUB Get_Sonar
                  'DEBUG CLS,DEC rawDist
                  GOSUB Forward_Pulse
                  PAUSE 21
          LOOP
        EXIT
        ENDIF
    NEXT

    PAUSE 20


    lcounter = lcounter + 1

  LOOP
  GOSUB Look_Straight
  PAUSE 60
  rawDist = init_large
  lcounter = 0
  DO  UNTIL rawDist <init_far + counter1
        DEBUG ? counter1
        IF lcounter = 1 THEN
          counter1 = 1000
        ENDIF
        GOSUB Get_Sonar
        'DEBUG CLS,DEC rawDist
        GOSUB Left_Pulse
        lcounter = lcounter +1
        PAUSE 21
  LOOP


LOOP





END



main1:
  DO
    DO  UNTIL rawDist <1200
        GOSUB Get_Sonar
        'DEBUG CLS,DEC rawDist
        GOSUB Left_Pulse
        PAUSE 21
    LOOP

    PAUSE 200
    rawDist = 2501
    DO  UNTIL rawDist <1200
        GOSUB Get_Sonar
        'DEBUG CLS,DEC rawDist
        GOSUB Right_Pulse
        PAUSE 21
    LOOP

    PAUSE 200

    DO  WHILE rawDist <1200
        GOSUB Get_Sonar
        'DEBUG CLS,DEC rawDist
        GOSUB Backward_Pulse
        PAUSE 21
    LOOP

  LOOP
RETURN

END
'-----[MAIN_END]---------------------------------------------

Get_Sonar:
                                   ' make trigger 0-1-0
  PULSOUT 11, 5                         ' activate sensor
  PULSIN  11, 1, rawDist                 ' measure echo pulse
  RETURN

Look_Left:
  FOR counter = 1 TO 24
    PULSOUT 15, 1500
    PAUSE 20
  NEXT
  PAUSE 20
RETURN

Look_Straight:
  FOR counter = 1 TO 24
    PULSOUT 15, 750
    PAUSE 20
  NEXT
  PAUSE 20
RETURN

Look_Right:
  FOR counter = 1 TO 24
    PULSOUT 15, 1
    PAUSE 20
  NEXT
  PAUSE 20
RETURN


Forward1:
  FOR counter = 1 TO 43
    PULSOUT 12, 850
    PULSOUT 13, 650
    PAUSE 20
  NEXT
  PAUSE 200
  RETURN

Forward3:                             'Move forward for 3 seconds
  FOR counter = 1 TO 122
    PULSOUT 12, 820
    PULSOUT 13, 650
    PAUSE 20
  NEXT
  PAUSE 200
  RETURN

Forward10:                            'Move forward 10 seconds (calibration)
  FOR counter = 1 TO 407
    PULSOUT 12, 850
    PULSOUT 13, 650
    PAUSE 20
  NEXT
  PAUSE 200
  RETURN

Forward_Pulse:                       'Move forward for a single pulse
  PULSOUT 12, 820
  PULSOUT 13, 650
  PAUSE 20
  RETURN

Backward1:
  FOR counter = 1 TO 43
    PULSOUT 12, 650
    PULSOUT 13, 850
    PAUSE 20
  NEXT
  PAUSE 200
  RETURN

Backward3:                           'Move backward for 3 seconds
  FOR counter = 1 TO 122
    PULSOUT 12, 680
    PULSOUT 13, 850
    PAUSE 20
  NEXT
  PAUSE 200
  RETURN

Backward10:                          'Move backward 10 seconds (calibration)
  FOR counter = 1 TO 407
    PULSOUT 12, 650
    PULSOUT 13, 850
    PAUSE 20
  NEXT
  PAUSE 200
  RETURN

Backward_Pulse:                      'Move backward for a single pulse
  PULSOUT 12,680
  PULSOUT 13, 850
  PAUSE 20
  RETURN

Right_Pulse:
  PULSOUT 13, 850
  PULSOUT 12, 850
  PAUSE 20
  RETURN

Right90:                             'Hard right 90 degrees
  FOR counter = 1 TO 20
    PULSOUT 13, 850
    PULSOUT 12, 850
    PAUSE 20
  NEXT
  PAUSE 200
  RETURN

RightRamp45:                          'Smooth right 45 degrees
 FOR counter = 1 TO 30
   PULSOUT 13, 750 + counter
   PULSOUT 12, 750 + counter
   PAUSE 20
 NEXT

 FOR counter = 1 TO 15
   PULSOUT 13, 765 - counter
   PULSOUT 12, 765 - counter
   PAUSE 20
 NEXT
 PAUSE 200
 RETURN

RightRamp90:                         'Smooth right 90 degrees
  FOR counter = 1 TO 30
    PULSOUT 13, 750 + counter
    PULSOUT 12, 750 + counter
    PAUSE 20
  NEXT

  FOR counter = 1 TO 30
    PULSOUT 13, 780 - counter
    PULSOUT 12, 780 - counter
    PAUSE 20
  NEXT
  PAUSE 200
  RETURN

RightRamp180:                        'Smooth right 180 degrees
  FOR counter = 1 TO 56
    PULSOUT 13, 750 + counter
    PULSOUT 12, 750 + counter
    PAUSE 20
  NEXT

  FOR counter = 1 TO 30
    PULSOUT 13, 780 - counter
    PULSOUT 12, 780 - counter
    PAUSE 20
  NEXT
  PAUSE 200
  RETURN

Left_Pulse:
  PULSOUT 13, 650
  PULSOUT 12, 650
  PAUSE 20
  RETURN

Left90:                              'Hard left 90 degrees
  FOR counter = 1 TO 20
    PULSOUT 13, 680
    PULSOUT 12, 650
    PAUSE 20
  NEXT
  PAUSE 200
  RETURN

LeftRamp45:                          'Smooth left 45 degrees
 FOR counter = 1 TO 30
   PULSOUT 13, 750 - counter
   PULSOUT 12, 750 - counter
   PAUSE 20
 NEXT

 FOR counter = 1 TO 15
   PULSOUT 13, 735 + counter
   PULSOUT 12, 735 + counter
   PAUSE 20
 NEXT
 PAUSE 200
 RETURN

LeftRamp90:                          'Smooth left 90 degrees
 FOR counter = 1 TO 30
   PULSOUT 13, 750 - counter
   PULSOUT 12, 750 - counter
   PAUSE 20
 NEXT

 FOR counter = 1 TO 28
   PULSOUT 13, 714 + counter
   PULSOUT 12, 714 + counter
   PAUSE 20
 NEXT
 PAUSE 200
 RETURN

LeftRamp180:                         'Smooth left 180 degrees
FOR counter = 1 TO 56
    PULSOUT 13, 750 - counter
    PULSOUT 12, 750 - counter
    PAUSE 20
  NEXT

  FOR counter = 1 TO 30
    PULSOUT 13, 715 + counter
    PULSOUT 12, 715 + counter
    PAUSE 20
  NEXT
  PAUSE 200
  RETURN

Jingle:                            'End Program Jingle
  FREQOUT 8, 25, 1865
  FREQOUT 8, 25, $92D
  FREQOUT 8, 25, $AE9
  FREQOUT 8, 50, 3728
  FREQOUT 8, 50, 4186
  RETURN