Boe-Bot with Ping and Compass

Parallax Boe-Bot roaming around with ping and compass to avoid obstacles.

Video

Hardware

Listing

Download bs2 file

'   Ping Sensor Roam

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

' -----[ Initialise LCD ]------------------------------------------------------
HIGH 11
PAUSE 100

' -----[ Initialise ServoPal ]-------------------------------------------------
INPUT 12
DO UNTIL 12
LOOP
LOW 12
PAUSE 100
HIGH 12

' -----[ Compass EEPROM Data ]-------------------------------------------------
CompassOffsets DATA @ 0, (4)                 ' Stores x and y axis offsets
CompassLowVal  DATA      (1)                 ' Stores index of lowest angle
CompassCal     DATA      (16)                ' 16 reference compass angles

' -----[ Compass Pin Definitions ]---------------------------------------------
DinDout        PIN       7                   ' P6 transceives to/from Din/Dout
Clk            PIN       6                   ' P5 sends pulses to HM55B's Clk
En             PIN       5                   ' P4 controls HM55B's /EN(ABLE)

' -----[ Compass Constants ]---------------------------------------------------
Reset          CON       %0000               ' Reset command for HM55B
Measure        CON       %1000               ' Start measurement command
Report         CON       %1100               ' Get status/axis values command
Ready          CON       %1100               ' 11 -> Done, 00 -> no errors
NegMask        CON       %1111100000000000   ' For 11-bit negative to 16-bits
current        CON       0                   ' Table array index
previous       CON       1                   ' Table array index

' -----[ Compass Variables ]---------------------------------------------------
x              VAR       Word                ' x-axis data
y              VAR       Word                ' y-axis data
status         VAR       Nib                 ' Status flags
angle          VAR       Word                ' Angle measurement
axisOffset     VAR       angle               ' Axis offset

index          VAR       Status              ' EEPROM index
table          VAR       Byte(2)             ' Stores EEPROM table values
span           VAR       x                   ' Span between table entries
angleOffset    VAR       y                   ' Offset btwn measured and table

' -----[ Ping Variables ]------------------------------------------------------
dr             VAR       Word                ' raw data from ping sensor
dm             VAR       Word                ' ping sensor data in mm
dm = 1000

longestdir     VAR       Word                ' longest direction
longestdist    VAR       Word                ' longest direction distance
currscanangle  VAR       Word                ' scan angle
target         VAR       Word                ' target angle

' -----[ Drive Variables ]-----------------------------------------------------
repeat         VAR       Byte                ' repeat, used for various features
counter        VAR       Byte                ' loop counter

' -----[ Scan Variables ]------------------------------------------------------
scanpos        VAR       Nib                 ' current position of scan

' -----[ Start Warning Sound ]-------------------------------------------------
FOR repeat = 1 TO 4
  FREQOUT 4, 100, 3000
  PAUSE 200
NEXT

' -----[ Main Loop ]-----------------------------------------------------------
main:
  dm = 500                                            ' set default distance to 500mm
  GOSUB get_distance                                  ' get actual distance
  IF dm > 0 AND dm < 350 THEN                         ' if distance > 0 and < 300 then obstruction found
    PULSOUT 12, 2000                                  ' halt one wheel drive motors
    PULSOUT 12, 2000                                  ' halt other wheel drive motor
    PAUSE 1000                                        ' pause for 1 second
    SEROUT 11, 32, [$0C]                              ' clear lcd display
    PAUSE 10                                          ' pause for 0.01 seconds (10 milliseconds)
    SEROUT 11, 32, [$16]                              ' turn on display with no cursor
    SEROUT 11, 32, [$11]                              ' turn on backlight
    SEROUT 11, 32, ["Dist ", DEC dm, " mm"]           ' display detected distance
    GOSUB get_heading                                 ' get current heading
    SEROUT 11, 32, [$0D]                              ' carriage return
    SEROUT 11, 32, ["Heading ", DEC angle]            ' display detected distance
    PAUSE 1000                                        ' pause for 1 second
    SEROUT 11, 32, [$12]                              ' turn off backlight

    ' scan 90 degrees left to 90 degrees right and decide direction to travel
    ' 90 degrees right
    FOR counter = 0 TO 20                             ' move ping to right 90 degrees
      PULSOUT 14, 300
      PAUSE 20
    NEXT

    longestdist = 0                                   ' reset longest distance to zero
    FOR currscanangle = 300 TO 1200 STEP 100          ' step through ping angles from 90 right to left
      FOR counter = 0 TO 10
        PULSOUT 14, currscanangle
        PAUSE 20
      NEXT
      GOSUB get_distance                              ' get actual distance
      PAUSE 150
      IF dm > longestdist THEN                        ' if dist measured > longest
        longestdir = currscanangle                    '  remember longest direction
        longestdist = dm                              '  remember longest distance
      ENDIF
    NEXT
    currscanangle = 650-(longestdir/5)                ' convert to required value

    ' rotate ping to best angle
    FOR counter = 0 TO 25
      PULSOUT 14, longestdir
      PAUSE 20
    NEXT
    PAUSE 1000

    ' calculate new heading
    GOSUB get_heading                                 ' get current angle
    target = currscanangle + angle                    ' target angle
    IF target < 500 THEN
      target = target + 360
    ENDIF
    IF target > 860 THEN
      target = target - 360
    ENDIF
    SEROUT 11, 32, [$0C]                              ' clear lcd display
    SEROUT 11, 32, [$11]                              ' turn on backlight
    SEROUT 11, 32, ["Curr Angle ", DEC angle]         ' current angle (+500)
    SEROUT 11, 32, [$0D]                              ' move to second line
    SEROUT 11, 32, ["Target  ", DEC target - 500]     ' target angle (+500)
    PAUSE 1000

    ' rotate to new heading
    IF angle + 500 >= target THEN
       DO
        PULSOUT 12, 740                               ' spin left
        PULSOUT 12, 740
        GOSUB get_heading
      LOOP WHILE angle + 500 > target
    ELSE
       DO
        PULSOUT 12, 760                               ' spin right
        PULSOUT 12, 760
        GOSUB get_heading
      LOOP WHILE angle + 500 < target
    ENDIF

    PULSOUT 12, 2000                                  ' halt one wheel drive motors
    PULSOUT 12, 2000                                  ' halt other wheel drive motor
    SEROUT 11, 32, [$0C]                              ' clear lcd display
    SEROUT 11, 32, [$11]                              ' turn on backlight
    GOSUB get_heading
    SEROUT 11, 32, ["New Hdg ", DEC angle]            ' display new heading
    PAUSE 1000

    SEROUT 11, 32, [$12]                              ' turn off backlight
    SEROUT 11, 32, [$15]                              ' turn off display
  ELSE
    PULSOUT 12, 720                                   ' if distance >= 300 then send drive to motor
    PULSOUT 12, 784                                   ' send drive to other motor
  ENDIF




  scanpos = scanpos + 1
  IF scanpos = 4 THEN scanpos = 0
  repeat = 20
  FOR counter = 0 TO repeat
    IF scanpos = 0 THEN PULSOUT 14, 650
    IF scanpos = 1 THEN PULSOUT 14, 750
    IF scanpos = 2 THEN PULSOUT 14, 850
    IF scanpos = 3 THEN PULSOUT 14, 750
    PAUSE 20
  NEXT

GOTO main
END


' -----[ Sub to Get Ping Distance ]---------------------------------------------------
get_distance:
  LOW 15
  PULSOUT 15, 5
  PULSIN 15, 1, dr
  dm = dr ** 22570
RETURN

' -----[ Sub to Get Heading Angle ]---------------------------------------------------
get_heading:
  HIGH En: LOW En                            ' Send reset command to HM55B
  SHIFTOUT DinDout,clk,MSBFIRST,[Reset\4]

  HIGH En: LOW En                            ' HM55B start measurement command
  SHIFTOUT DinDout,clk,MSBFIRST,[Measure\4]
  status = 0                                 ' Clear previous status flags

  DO                                         ' Status flag checking loop
    HIGH En: LOW En                          ' Measurement status command
    SHIFTOUT DinDout,clk,MSBFIRST,[Report\4]
    SHIFTIN  DinDout,clk,MSBPOST,[Status\4]  ' Get Status
  LOOP UNTIL status = Ready                  ' Exit loop when status is ready

  SHIFTIN  DinDout,clk,MSBPOST,[x\11,y\11]   ' Get x & y axis values
  HIGH En                                    ' Disable module

  IF (y.BIT10 = 1) THEN y = y | NegMask      ' Store 11-bits as signed word
  IF (x.BIT10 = 1) THEN x = x | NegMask      ' Repeat for other axis

  READ CompassOffsets, Word axisOffset           ' Get x-axis offset
  x = x - axisOffset                             ' Correct x-axis
  READ CompassOffsets + 2, Word axisOffset       ' Get y-axis offset
  y = y - axisOffset                             ' Correct y-axis

  angle = x ATN -y                           ' Convert x and y to brads

  READ CompassLowVal, index

  READ CompassCal + index, table(current)
  READ (CompassCal + (index - 1 & $F)), table(previous)

  IF (angle >= table(previous)) THEN
    span = (255 - table(previous)) + table(current)
    angleOffset = angle - table(previous)
  ELSEIF (angle <= table(current)) THEN
    span = table(current) + (255 - table(previous))
    angleOffset = angle + (255 - table(previous))
  ELSE
    index = index - 1
    READ CompassCal + index, table(current)
    DO
      table(previous) = table(current)
      index = index + 1
      READ CompassCal + index, table(current)
      IF (angle <= table(current)) AND (angle > table(previous)) THEN
        span = table(current) - table(previous)
        angleOffset = angle - table(previous)
        EXIT
      ENDIF
    LOOP
  ENDIF

  angleOffset = angleOffset * 16
  angle = (angleOffset / span) + ((angleOffset // span) / (span / 2))
  angle = ((index - 1 & $F) * 16) + angle
  angle = angle & $ff

  angle = angle */ 360
RETURN

Home Site Map Licence and Warranty