-- commands - execute commands received on the serial line
-- letter commands
const byte cleft = "l"
const byte chleft = "k"
const byte cstraight = "t"
const byte chright = "q"
const byte cright = "r"
const byte cforward = "f"
const byte creverse = "r"
const byte cstop = "s"
const byte cstart = "o"
const byte clookaround = "a"
const byte clook = "l"
const byte cdrive = "d"


-- typical commands: flB -> forward left 2
-- rqa -> reverse half right 26

include f877_20
include jlib
include f877_modules
include const
include upins
include var
include com
include steer
include polaroid
include routines
include behave

procedure plookaround is
if ddir != stop then
com("x")
else
setstart
speed = 0
ddir = stop
end if
end procedure



procedure pdrive is
dmode = dmodecontrolled
if serin == cforward then
if ddir == forward | ddir == stop then
ddir = forward
end if
elsif serin == creverse then
if ddir == reverse | ddir == stop then
ddir = reverse
end if
end if

-- direction

serin = comwait
-- if serin == cleft then
-- steer = sleft
-- elsif serin == chleft then
-- steer = shleft
-- elsif serin == cstraight then
-- steer = smiddle
-- elsif serin == chright then
-- steer = shright
-- elsif serin == cright then
-- steer = sright
-- end if
steer = serin
com(serin)

-- distance
serin = comwait
drivecount = serin - "A"
com3(drivecount)
speed = minspeed
for 15 loop
setsteer
delay_10ms(2)
end loop
beep
end procedure

procedure plook is
serin = comwait
polservo(serin)
speed = 0
ddir = stop
end procedure

procedure pmode is
com(serin)
serin = comwait
if serin == "f" then
dmode = dmodefree
elsif serin == "c" then
dmode = dmodecontrolled
elsif serin == "m" then
gp2d12
getpol
com("i") com3(dist)
com("p") com3(pdist)
end if
com(serin)
end procedure


procedure commands is
-- acknowledge

com(serin)
initstart
if serin == cstop then
-- come to a stop
ddir = stop
stuck = stuckcommand
elsif serin == cforward | serin == creverse then
pdrive
elsif serin == clookaround then
plookaround
elsif serin == clook then
plook
elsif serin == cdrive then
pmode
elsif serin == cstart then
dmode = dmodefree
setstart
else
com(serin)
end if

end procedure