Help with PING sensor and plotting.

Started by dosun129 April 16, 2008
I have a BX24p in a Board of education with a PING sensor and eb500.
Currently, i have the robot roaming with the PING sensor and what i
want to do is, every time the PING sensor sees an object close
enough (say 5inches) i want it to plot to stampplot via the eb500
bluetooth module.
Independently, i have 2 programs to (1)plot XY values to stampplot
via bluetooth, and (2)roam with PING.
Now, i tried to integrate the two which is where i have problems!
So this is what i thought would work
(a) I would scan with sensor, get values x and y, open ports by
using definecom3,openqueue,opencom but when i do that, it doesn't
work as the robot just hangs. Initially, the robot moves, and then
when it sees an object close enough, it stops everything and does
So then i tried another way
(b) open port, then try to scan and get the values x and y to
transfer via Bluetooth. But in this case, after i open the port for
bluetooth, the PING does not scan, it just hangs and my servo that
is on my ping sensor sounds like it is receiving commands cuz i hear
a squeal.

Below is my code. Hopefully someone can help me out!!
Option Explicit

'dim getsonarstack(1 to 250) as byte
Dim Width as Single, Inches as Single, feet as single
dim xcoord as single
dim ycoord as single
dim sweepcount as single
dim pulsecount as integer
dim counter as integer
dim degrees as single
dim rad as single
'dim forwardstack(1 to 80) as byte
dim xout(1 to 100) as byte
dim ascii_dec as byte
dim ascii_minus as byte
dim xin(1 to 2) as byte

Sub Main()

call GetSonar()

End Sub

public sub GetSonar()
dim counter as integer
dim x as integer


for counter = 0 to 6 'move ping sensor right to left
sweepcount = 0.00065+(0.00028*(Csng(counter)))
for x = 0 to 7
call pulseout(19,sweepcount,1)
call scan(sweepcount)
for counter = 0 to 5 'move ping sensor left to right.
sweepcount = 0.00215-(0.00028*(Csng(counter)))
for x = 0 to 8
call pulseout(19,sweepcount,1)
call scan(sweepcount)

end sub

public sub scan(byval sweepcount as single)
dim comma as byte
dim cr as byte

comma = 44
Call PutPin(20, 0)
Call PulseOut(20, 3.0, 1) 'send 3V to ping
Call PulseIn(20, 1, Width) 'receive
If Width <= 0.0 Then
' Debug.Print "Nothing Found"
Feet = Width * 550.0 ' 1100 feet per sec
Inches = feet * 12.0

degrees = (sweepcount-0.00065)*100000.0
'0.00065 gives 0 degree position for ping

'0.0023 gives ~165 degree position for ping

'every 0.0001 pulse = 10 degrees.

'x10000 for whole #. x10 for degrees. = 100 000
'debug.print "degrees = "; Cstr(degrees)
rad = degrees*0.0174532925
xcoord = inches*cos(rad)
'xcoord to plot
ycoord = inches*sin(rad)
'ycoord to plot
if inches < 5.0 then
call definecom3(0,6,bx0000_1000)
call openqueue(xout,100)
call openqueue(xin,2)
call clearqueue(xout)
call clearqueue(xin)
call opencom(3,9600,xin,xout)
call split(xcoord)
'convert xcoord single to byte and then putqueue
into xout.
call putqueue(xout,comma,1)
'put comma after xcoord.
call split(ycoord)
'convert ycoord to byte
call putqueue(xout,cr,1)
'carriage return
delay 0.5

'call clearqueue(xout)
'call closecom(3,xin,xout)
if sweepcount < 0.00134 then
call go_left()
elseif sweepcount > 0.00134 then
call go_right()
call go_backward()

end if
call go_forward()
end if

End If
end sub
public sub split(byval value as single)

dim temp as byte
dim left as single
dim right as single
dim abRight as single
dim hold as integer
dim hold2 as single
dim strLeft as string
dim strRight as string
dim chLeft as byte
dim chRight as byte
dim ascii_zero as byte
hold = Cint(value)
hold2 = Csng(hold)
if (value < hold2) then 'subtract 1 because Cint round up.
end if
right = 10.0*(value - abs(hold2)) 'to the right of decimal
lefts(value) 'to the left
of decimal
strLeft = Cstr(left)
strRight = Cstr(abright)
if (value < 0.0) then 'if value is negative
call putqueue(xout,ascii_minus,1) 'add minus sign.
call putqueue(xout,chleft,1) 'left of decimal
call putqueue(xout,ascii_dec,1) 'decimal point
if (right = 0.0) then
call putqueue(xout,ascii_zero,1)
call putqueue(xout,chright,1)
end if

call putqueue(xout,chleft,1)
call putqueue(xout,ascii_dec,1)
if (right = 0.0) then
call putqueue(xout,ascii_zero,1)
call putqueue(xout,chright,1)
end if
end if
end sub