DEMO SOURCE CODE FOR SCRIBBLER HACK SIDEBAR

' {$STAMP BS2}
' {$PORT COM2}
' {$PBASIC 2.5}

'
' Using the Devantech SRF04 sensor with the Scribbler Robot
'

distance VAR Word

trigger CON 10
echo CON 9

convfac CON 74 ' use inches

MotorRight PIN 12
MotorLeft  PIN 13
Stall PIN 7

INPUT 7
LOW MotorRight
LOW MotorLeft

 

'---------------------------------------------

main:

 

        IF (stall = 1) THEN              
           PULSOUT MotorRight, 1500       ‘Reverse motors …
           PAUSE 1                        ‘ … for …
           PULSOUT MotorRight, 2200       ‘ 3 seconds
           PULSOUT MotorLeft, 1200        ‘ Reverse left motor at a slower speed
           PAUSE 1                        ‘ for …
           PULSOUT MotorLeft, 2200        ‘ 3 seconds
           DEBUG "STALL", CLS
        ENDIF

        IF (distance < 7) THEN            ‘ if an obstacle is very close
           PULSOUT MotorRight, 1500       ‘ reverse both wheels
           PAUSE 1
           PULSOUT MotorRight, 2200
           PULSOUT MotorLeft, 1500
           PAUSE 1
           PULSOUT MotorLeft, 2200

        ELSEIF (distance < 15) THEN       ‘ hard turn
          PULSOUT MotorRight, 2500
          PULSOUT MotorLeft, 3000

        ELSEIF (distance < 18) THEN       ‘medium turn
          PULSOUT MotorRight, 2600
          PULSOUT MotorLeft, 3000

        ELSEIF (distance < 20) THEN       ‘shallow turn

          PULSOUT MotorRight, 2800
          PULSOUT MotorLeft, 3000

        ELSE                              ‘drive straight forward
          PULSOUT MotorRight, 2900
          PULSOUT MotorLeft, 3000

        ENDIF

 

        GOSUB ultrasonic                  ‘read the ultrasonic sensor
        DEBUG DEC distance, CR            ‘display the distance read from the sensor
        PAUSE 20
        GOTO main

 

ultrasonic:                         ‘ code from Acroname

        PULSOUT INIT,5              ' 10us init pulse
        OUTPUT INIT                       ' dummy command (delay)
        RCTIME ECHO,1,distance            ' measure echo time
        distance = distance/convfac             ' convert to inches
        PAUSE 10

               

Based on example code from Acroname (http://acroname.com/robotics/info/examples/srf04-1/srf04-1.html)