Unimog BEHAVE.JAL

behave smart

include irsens
include setf

-- behaviour routines


const bit dophotoinit = off


procedure env65 ( byte in mode) is
-- set steer according to environment pol 6500
-- polm
-- poll polr
--
-- polll polrr
--
--
lsteer = steer

-- first look for best value
if mode == "s" then
if poll > pgo | polm > pgo | pdist > pgo | polr > pgo then

if (( polr / 2 + polrr / 2 ) < ( poll / 2 + polll / 2 )) then
lsteer = shleft
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","1") com3(lsteer) end if
else
lsteer = shright
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","2") com3(lsteer) end if
end if
if polll > polrr * 2 then
lsteer = sleft
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","3") com3(lsteer) end if
elsif polrr > polll * 2 then
lsteer = sright
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","4") com3(lsteer) end if
end if
if polm > pgo then
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","5") com3(lsteer) end if
lsteer = smiddle
end if
else
-- no extra good value available
-- check left better than right and so on
if poll > polr then
-- left looks better
if polm < pgo then
lsteer = sleft
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","6") com3(lsteer) end if
else
lsteer = shleft
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","7") com3(lsteer) end if
end if
elsif polr > pgo then
-- right looks better
if polm < pgo then
lsteer = sright
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","8") com3(lsteer) end if
else
lsteer = shright
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","9") com3(lsteer) end if
end if
end if

-- check for passage

if polll < polm & polrr < polm &
poll < polm & polr < polm then
lsteer = smiddle
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","p") com3(lsteer) end if
end if
end if
-- best value polaroid
if polv > pgo then
-- good value available
if polp < poshr then
lsteer = sright
elsif polp < posm then
lsteer = shright
elsif polp < poshl then
lsteer = shleft
elsif polp < posl then
lsteer = sleft
end if
elsif foundv < dtrigger then
-- at least one value < dtrigger...
if foundp < poshr then
lsteer = sright
elsif foundp < posm then
lsteer = shright
elsif foundp < poshl then
lsteer = shleft
elsif foundp < posl then
lsteer = sleft
end if
end if
steer = lsteer
elsif mode == "d" & dmode == dmodefree then
-- driving and auto steering
if pdist < pstop then
if avgpa > pstop then
-- something close and better values available
if ppoint > posm + pforward / 2 then
-- we're looking left
lsteer = sright
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","a") com3(lsteer) end if
elsif ppoint < posm - pforward / 2 then
lsteer = sleft
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","b") com3(lsteer) end if
end if
end if
elsif pdist < pgo then
-- something around
if ppoint > posm + pforward / 2 then
-- we're looking left
lsteer = shright
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","c") com3(lsteer) end if
elsif ppoint < posm - pforward / 2 then
-- we're looking right
lsteer = shleft
-- if debugenv65 == 1 & noserial == 0 then show_values("e","6","d") com3(lsteer) end if
end if
end if

if lsteer < steer then
steer = steer - steerstep
elsif lsteer > steer then
steer = steer + steerstep
end if
lightsteer
if debuglight == 1 & noserial == 0 then
show_values("l","s","t")
end if
end if

if debugenv65 == 1 & noserial == 0 then show_values("e","n","6") end if
end procedure

procedure envgp2d12 ( byte in mode ) is
-- set steer according to environment gp2d12
-- foundm
-- foundl foundr
--
-- foundll foundrr
--
--
-- ppoint dist

if nogp2d12 == 0 then

if dist < dtrigger then
-- nothing to do here
elsif dist > dtrigger & dist <= ( dtrigger + dmax ) / 3 then
-- something within view - take evasive action
if debugcheckset == 1 then com("S") end if
if speed > midspeed then
speed = minspeed
end if
elsif dist > (dtrigger + dmax ) / 3 & dist <= dmax then
-- quite close - slow down
if speed > minspeed then
speed = minspeed
end if
if debugcheckset == 1 then com("L") end if
elsif dist > dstop & dist < dmax then
if debugcheckset == 1 then com("s") com("t") com("o") com("p") end if
ddir = stop
breakled = on
stuck = stuckgp2d12
if speed > midspeed then
speed = 0
else
speed = minspeed
end if
elsif dist > dmax then
-- too close - reverse !
mf = low
mr = low
ddir = reverse
drivecount = reverseconst
speed = minspeed

drivecount = reverseconst
if debugcheckset == 1 then com("r") com("e") com("v") com("!") end if
end if

if debugcheckset == 1 & noserial == 0 then
show_values("e","g","e")
end if

-- check left better than right and so on
if foundl > foundr & foundll > foundrr & ( foundl > foundm | foundll > foundm ) then
-- left looks better
if foundm < dstop then
lsteer = sleft
else
lsteer = shleft
end if
elsif foundl < foundr & foundll < foundrr & ( foundr > foundm | foundrr > foundm ) then
-- right looks better
if foundr < dstop then
lsteer = sright
else
lsteer = shright
end if
end if

-- check for passage

if foundll < foundm & foundrr < foundm &
foundl < foundm & foundr < foundm then
lsteer = smiddle
end if
if mode == "s" then
steer = lsteer
elsif mode == "d" & dmode == dmodefree then
steer = steer / 2 + lsteer / 2
end if
end if
end procedure




procedure setr is
-- just reverse until tacho reaches drivecount -
-- reverse sensor not ready yet, and it does not make sense to check front...

-- check sensors and set values direction reverse
if debugcheckset == 1 & noserial == 0 then
show_values("c","s","r")
end if
if revsens > revsensconst then
-- the reverse sensor saw something
stuck = stuckrev
ddir = stop
breakled = on
if revsens > irgenloop / 2 then
-- quite close!
mf = low
mr = low
speed = 0
end if
else
-- assume nothing is at the back
if dmode == dmodecontrolled then
if tacho >= drivecount then
stuck = stucktacho
ddir = stop
breakled = on
if noserial == 0 then com("C") end if
end if
else
if nogp2d12 == 0 then
if dist > dtrigger then
if dist > dstop then
-- something close - reverse more
drivecount = reverseconst
speed = minspeed
if dmode == dmodefree then
-- tacho = 0
end if
else
-- something there, but no too close
-- just continue reversing
-- drivecount = reverseconst
speed = minspeed
end if
end if
end if
-- polaroid
if pdist < ptrigger then
-- something close - reverse more
drivecount = reverseconst
speed = minspeed
if dmode == dmodefree then
-- tacho = 0
end if
end if
if tacho >= drivecount then
stuck = stucktacho
ddir = stop
speed = 0
breakled = on
end if
if noserial == 0 then com("C") end if
end if
end if
if speed > ( minspeed + midspeed) / 2 & tacho > 0 then
-- don't reverse too fast
speed = minspeed
end if

-- handle speed
if revsens > 0 then
stuck = stuckrev
ddir = stop
breakled = on
else
end if

if debugcheckset == 1 & noserial == 0 then
show_values("c","s","v")
end if
end procedure

procedure sets is

-- check sensors and set values direction stop
-- we are not moving - look around and set start values
if speed > 0 & ( dist > dstop | revsens > revsensconst ) then
-- stop fast
mf = low mr = low
speed = 0
stuckcounter2 = 0
stuckcounter = 0
setspeed
if debugsound == 1 then
if nogp2d12 == 0 then
if dist > dstop then
testled = on
beep2
end if
if dist > dmax then
beep2
end if
end if
if pdist < pstop then
frontled = on
beep1
end if
if pdist < ptrigger then
beep1
end if
end if
elsif speed > minspeed then
speed = speed - speedstep * 3
if speed < minspeed then
speed = 0
end if
end if

if debugcheckset == 1 & noserial == 0 then
show_values("c","s","s")
end if
end procedure


procedure evaldrive is
-- what to do when driving
-- ddir & speed & steer

-- calculate average value of polaroid sensor
avgpa = ( poll / 3 + polm / 3 + polr / 3 ) / 2 + pdist / 2

-- same thing here for the gp2d12
if nogp2d12 == 0 then
avgia =( foundl / 3 + foundm / 3 + foundr / 3 ) / 2 + dist / 2
end if

if debugcheckset == 1 then
com("c") com("f")
com3(ppoint)
if nogp2d12 == 0 then
com3(dist)
end if
com3(pdist)
end if

-- ok, let's calculate
-- SPEED

if valdebug == 1 & noserial == 0 then
show_values("e","v","d")
end if
if ddir == forward then
setf

-- continue driving if everything ok

if noremote == high & driveallthetime == 1 & tacho + 5 >= drivecount then
tacho = drivecount - 5
elsif tacho >= drivecount then
ddir = stop
stuck = stucktacho
breakled = on
end if

elsif ddir == stop then
sets
elsif ddir == reverse then
setr
end if

if valdebug == 1 & noserial == 0 then
show_values("e","v","2")
end if



end procedure

procedure evalstop is
-- check bumpers
bumpersoff
-- known values are IR Sensor foundm -l -ll -r -rr -p -v

ddir = forward
speed = minspeed * 2

if pdist > ptrigger then
env65("s")
end if
if nogp2d12 == 0 then
if dist < dstop then
envgp2d12("s")
end if
end if

speed = minspeed
if nogp2d12 == 0 then
if foundm > dstop | foundl > dstop | foundr > dstop then
ddir = reverse
end if
end if
if polr < pstop | polm < pstop | poll < pstop |
foundm > dstop | foundl > dstop | foundr > dstop then
ddir = reverse
end if
if ddir == reverse then
drivecount = reverseconst
if polll > polrr & foundrr > dtrigger then
-- left more space, right enough space to turn
steer = sright
elsif polll < polrr & foundrr > dtrigger then
steer = sleft
else
steer = smiddle
end if
end if
-- set it blind -

end procedure

procedure set is
-- test cny70 - pulses from wheel movement
if testcny == 1 then
if ddir > stop then
-- we are supposed to be driving
if stuckcounter == 0 then
-- nothing moved - count it
stuckcounter2 = stuckcounter2 + 1
if speed < minspeed then
speed = speed + stuckcounter2
elsif speed < maxspeed - speedstep then
speed = speed + speedstep
end if
else
if noserial == 0 & debugstuck == 1 then
com("s") com("c") com3(stuckcounter)
end if
if stuckcounter > 1 then
-- we're driving - slow down a bit so I can keep track of what's going on
-- this helps when you have different batteries -
-- the vehicle won't speed away like mad
if speed > minspeed then
speed = speed - stuckcounter
end if
com("s") com("s") com3(speed)
end if
stuckcounter2 = 0
end if
if stuckcounter2 > stuckconst then
-- give it some time to start off - stop if nothing moves
-- stop
testled = on
stuck = stuckblock
ddir = stop
breakled = on
speed = 0
if debugcheckset > 0 then show_values("0","s","t") end if
drivecount = driveconst
stuckcounter2 = 0
tacho = 0
end if
end if
stuckcounter = 0
end if

if speed > 0 then
-- get sensor values
pspoint
if nogp2d12 == 0 then
gp2d12
end if
getpol
evaldrive
end if

if debugcheckset == 1 & noserial == 0 then
show_values("s","e","e")

end if
end procedure



procedure setstart is
show_stuck
initstart
if noremote == high then
for 40 loop
com("*")
end loop
end if

pollook
ddir = forward
drivecount = driveconst
evalstop
if noserial == 0 then
end if


beep
com(13) com(10)
com("L")
com("i")
com("l") com3(foundll)
com("h") com3(foundl)
com("m") com3(foundm)
com("h") com3(foundr)
com("r") com3(foundrr)
com("b") com3(foundv)
com("p") com3(foundp)

com("p")
com("l") com3(polll)
com("h") com3(poll)
com("m") com3(polm)
com("h") com3(polr)
com("r") com3(polrr)
com("b") com3(polv)
com("p") com3(polp)

com("c") com3(drivecount)
com("s") com3(steer)
com("d") com(ddir+"0")
com("z")
com(13) com(10)
bothposi(posm)

photo_init -- take new start values
if dmode == dmodefree then
if nogp2d12 == 0 then
gp2d12
end if
getpol
end if

end procedure