Dual ping sensor navigation

knd775

I am using two ping sensors, one mounted on the front and one mounted on the left, to navigate around a box, walled with 2x4s, and pick up 12 golf balls in predefined locations. I am having problems writing the code to keep it a set distance (24cm) from the wall on the left. Theoretically, this should work, I think, but I don't know quite enough about PBasic to know if everything I am doing is correct/ possible.Here is what I have at the moment:

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

' Team QuickPick
' Code by ----- -------

pulseCount   VAR     Byte
runNumber    VAR     Byte

correctPulses  VAR  Byte
'angleResult    VAR  Byte

CmConstant   CON     2260
InConstant   CON     890

cmDistanceFront   VAR     Word
inDistanceFront   VAR     Word
timeFront         VAR     Word

cmDistanceSide  VAR     Word
inDistanceSide  VAR     Word
timeSide        VAR     Word

FREQOUT 11, 125, 5000                        ' Signal program start/reset with piezo buzzer.
PAUSE 1500
DO
  runNumber = runNumber + 1                  ' Adds 1 to "runNumber" counter
  GOSUB forwardUntilWall
  GOSUB turnRight
  LOOP UNTIL runNumber = 99                  ' Loops so that program runs X number of times
END


'[************]
'[Sub-Routines]
'[************]
forward:                                     ' Forward subroutine.
  FOR pulseCount = 1 TO 122
    PULSOUT 13, 850
    PULSOUT 12, 650
    PAUSE 20
  NEXT
  RETURN

forwardUntilWall:                            ' Forward with ping))) subroutine.
  DO
  GOSUB checkDistanceFront
  PAUSE 20
  GOSUB checkDistanceSide
  IF cmDistanceFront < 36 THEN
    RETURN
  ELSEIF (cmDistanceSide >= 24) OR (cmDistanceSide <= 26) THEN
    GOSUB correctDirection
  ELSEIF cmDistanceFront > 36 THEN
    PULSOUT 13, 850
    PULSOUT 12, 650
  ENDIF
  LOOP
  RETURN

backward:                                    ' Backward subroutine.
  FOR pulseCount = 1 TO 122
    PULSOUT 13, 650
    PULSOUT 12, 850
    PAUSE 20
  NEXT
  RETURN

turnLeft:                                    ' Left turn subroutine.
  FOR pulseCount = 1 TO 29
    PULSOUT 13, 650
    PULSOUT 12, 650
    PAUSE 20
  NEXT
  RETURN

turnRight:                                   ' right turn subroutine.
  FOR pulseCount = 1 TO 29
    PULSOUT 13, 850
    PULSOUT 12, 850
    PAUSE 20
  NEXT
  RETURN

checkDistanceFront:
  PULSOUT 10, 5
  PULSIN 10, 1, timeFront

  cmDistanceFront = cmConstant ** timeFront
  inDistanceFront = inConstant ** timeFront

  DEBUG HOME, DEC3 cmDistanceFront, " cm (front)"
  DEBUG CR, DEC3 inDistanceFront, " in"

  RETURN

checkDistanceSide:
  PULSOUT 9, 5
  PULSIN 9, 1, timeSide

  cmDistanceSide = cmConstant ** timeSide
  inDistanceSide = inConstant ** timeSide

  DEBUG HOME, DEC3 cmDistanceSide, " cm (side)"
  DEBUG CR, DEC3 inDistanceSide, " in"

  RETURN

correctDirection:
  GOSUB checkDistanceSide
  IF ((cmDistanceSide - 25) > 0) THEN
    'angleResult = ((inDistanceSide - 10) /
    FOR pulseCount = 1 TO 2
      PULSOUT 13, 850
      PULSOUT 12, 850
      PAUSE 20
    NEXT
  ELSEIF ((cmDistanceSide - 25) < 0) THEN
    FOR pulseCount = 1 TO 2
      PULSOUT 13, 650
      PULSOUT 12, 650
      PAUSE 20
    NEXT
  ENDIF

Any help would be greatly appreciated.

erco

Is this for a BoeBot?

knd775

Yes, it is. I apologize for not replying sooner. My notification settings are weird...