-- check Polaroid 6500
-- this controls the servo with the cam and the polaroid sonar
-- also the front sensor with the gp2d12

include f877_20
include jlib
include f877_modules
include const
include upins
include var
include const
include com
include routines

procedure getpol is
-- init
-- make sure we get the all important shots
if ppoint + 1 > poshl & ppoint - 1 <= poshl then
polcount = posconst
elsif ppoint + 1 > posl & ppoint - 1 <= posl then
polcount = posconst
elsif ppoint + 1 > posm & ppoint - 1 <= posm then
polcount = posconst
elsif ppoint + 1 > posr & ppoint - 1 <= posr then
polcount = posconst
elsif ppoint + 1 > poshr & ppoint - 1 <= poshr then
polcount = posconst
end if
if dopolpulse == 1 & polcount == posconst then
polcount = 0
polcount = polcount + 1
polbinh = low
polinit = high
pdist = 0
t = 0
delay_1ms
polbinh = high
while polecho == low & t < 11 loop
-- 29ms =~ 1 cm??
delay_1us(29)
pdist = pdist + 1 -- 200 passes here =~ 1m
if pdist == 200 then
t = t + 1
pdist = 0
end if
end loop
if t > 2 | t == 2 & pdist > 100 then
if t > 4 then
pdist = 255
else
pdist = 200 + t * 10 + pdist / 20
end if
else
-- cm
pdist = t * 100 + pdist / 2
pdist = pdist + poffset
end if
if noserial == 0 & debug6500 == 1 then
com3(pdist) com(13) com(10)
end if
polinit = low
polbinh = low
if ( valdebug == 1 | debugpol == 1& noserial == 0 ) then
show_values("p","o","l")
end if
else
-- skip this ping - save some current and time
polcount = polcount + 1
if ( valdebug == 1 | debugpol == 1& noserial == 0 ) then
show_values("p","o","+")
end if
end if
if ppoint == posl then
if debugsenval == 1 then com("u") com("l") com3(pdist) end if
polll = pdist
elsif ppoint == poshl then
if debugsenval == 1 then com("u") com("h") com3(pdist) end if
poll = pdist
elsif ppoint == posm then
if debugsenval == 1 then com("u") com("m") com3(pdist) end if
polm = pdist
elsif ppoint == poshr then
if debugsenval == 1 then com("u") com("h") com3(pdist) end if
polr = pdist
elsif ppoint == posr then
if debugsenval == 1 then com("u") com("r") com3(pdist) end if
polrr = pdist
end if
end procedure




procedure pollook is
polll = 0 poll = 0 polm = 0 polr = 0 polrr = 0
polp = 0 polv = 0
-- ir sensor

foundm = 0 foundl = 0 foundr = 0
foundll = 0 foundrr = 0
foundp = 0 foundv = 0

bothposi(posr)
delay_100ms(2) -- allow servos to swing out
ppoint = posr
while ppoint <= posl loop
-- sweep right to left, check environment
delay_20ms(2)
gp2d12
-- frequency(dist,16)
polcount = posconst -- make sure we do measure
getpol
if pdist > polv then
-- remember best pol value
polv = pdist
polp = ppoint
end if
if dist < foundv then
-- remember best gp2d12 value
foundv = dist
foundp = ppoint
end if
ppoint = ppoint + posstep
if debugpol == 1 & noserial == 0 then
com("p") com("o") com("l")
com3(pdist)
end if
bothservo
if showlook == 1 then
show_values("M","A","I")
end if
end loop

if noserial == 0 then
-- com("p") com("o") com3(polp);
-- com("v") com3(polv);
end if
end procedure


-- test procedure

-- f877_serial_setup ( 192 ) -- 19200 baud, 8,n,1 format.
--
-- forever loop
-- pollook
-- com(13) com(10) com3(polp) com3(polv) com(13) com(10)
-- com(13) com(10) com3(polpmin) com(13) com(10)
--
-- com("l") com3(polll) com("l") com3(poll) com("m") com3(polm)
-- com("r") com3(polr) com("r") com3(polrr) com(13) com(10)
--
-- com("l") com3(foundll) com("l") com3(foundl) com("m") com3(foundm)
-- com("r") com3(foundr) com("r") com3(foundrr) com(13) com(10)
-- end loop