set drive parameters jal code

procedure setf is

-- third version - complete rewrite

-- at this point, all sensor values are known and updated.

-- test from close to further away

if dist > dmax then

-- handle urgend stuff first
-- closeup situations
speed = 0
ddir = stop
stuck = stuckgp2d12
elsif pdist < dtrigger then
speed = 0
ddir = stop
stuck = stuckpolaroid
else
-- at this point we don't need to stop immediately, so we can
-- think about where to go
if ( pdist < pstop | dist > dstop ) then
-- something within the stop area - try do drive around
if pdist < pstop & dist < dstop then
-- sonar warning - gp2d12 says ok
if ppoint > posm then
steer = steer - steerstep
-- looking left - turn right
else
steer = steer + steerstep
end if
elsif pdist > pstop then
-- warning must have come from gp2d12
if ppoint > posm then
-- gp2d12 looks right here
steer = sleft
else
steer = sright
end if
end if
if speed > minspeed then
speed = minspeed
end if
elsif pdist < pgo then
if speed > minspeed then
speed = speed - speedstep
end if
elsif avgpa > pgo & dist < dtrigger then
if speed < midspeed then
speed = speed + speedstep
end if
-- free space - find out where :-)
-- if ppoint > posm - pforward / 2 & ppoint < posm then
-- -- looking right
-- if steer > shright then
-- steer = steer - steerstep
-- end if
-- elsif ppoint < posm + pforward / 2 & ppoint > posm then
-- -- looking left
-- if steer < shleft then
-- steer = steer + steerstep
-- end if
-- end if
end if
end if


if brakeon == 1 then
if speed > midspeed then
speed = midspeed
if debugcheckset == 1 & noserial == 0 then
show_values("b","r","k")
end if
end if
end if
if debugsetf == 1 & noserial == 0 then
show_values("s","e","f")
end if
end procedure