-- Used Libraries.
include f877_20
include jlib
include f877_modules
include const
include upins
include var
include com
include steer
include polaroid
include routines
include behave
include commands


-- Reserve ISR entry point
procedure interrupt_handler is
pragma interrupt
if intcon_intf then
asm incf stuckcounter, f
if stuckcounter == 0 then
asm incf stuckcounter, f
end if
if cny70 < cny70max then
asm incf cny70, f
else
cny70 = 1
asm incf tacho, f
end if
-- testled = on
intcon_intf = low
end if

end procedure



procedure waitgp2d12 is

if noserial == 0 then
com("w") com("a") com("i") com("t") com("g")
com("p") com("2") com("d") com("1") com("2")
end if

while ( gloop < 100 ) loop
if gloop > 100 then
gloop = 0
end if
gloop = gloop + 1
if nogp2d12 == 0 then
gp2d12
if dist > dstop then
gloop = 101
end if
end if
if revsens > 5 then
gloop = 101
end if
ppoint = posm
bothservo
delay_10ms
if gloop < 50 then
breakled = off
testled = on
frontled = off
else
breakled = on
frontled = on
testled = off
end if
end loop
gloop = gloopconst
end procedure

-- --------------------------------------------------------------------
-- Start of program
-- --------------------------------------------------------------------
if noremote == high then
f877_serial_setup ( 192 )
end if
ddir = stop
speed = 0
drivecount = 0
stuck = stuckinit
cny70 = 0
tacho = 0
ppoint = posm
dmode = dmodefree
-- init
steer = smiddle
bothposi(posm)
setsteer
driverc


-- mark beginning of program on log
if noserial == 0 & noremote == high then
for 80 loop
com("-")
end loop
end if

dist = 0
if noserial == 0 & noremote == high then
com("d") com("s") com("t") com("o") com("p") com3(dstop)
com("d") com("i") com("s") com("t") com3(dist) com(13) com(10)
end if
beep2
ddir = forward
speed = maxspeed / 4 -- default miminum speed
mf = low
mr = high
-- define minimum speed for move
-- make sure we can drive - adapts power for battery and floor changes
stuckcounter = 0
drivecount = reverseconst
tacho = 0

-- set up interrupt for cny70 - detect wheel motion
intcon_inte = high -- Enable RB0/Int (Index)
intcon_gie = high -- Enable interrupts
intcon_intf = low -- Clears RB0/Int flag
beep

while tacho < 3 & speed < maxspeed loop
speed = speed + speedstep
setspeed
if noserial == 0 & noremote == high then
show_values("t","s","p")
end if
delay_10ms
end loop
mr = low
if speed < maxspeed then
minspeed = speed / 2 + speed / 4
-- midspeed = speed
midspeed = maxspeed / 2 + minspeed / 2
if noserial == 0 & noremote == high then
com("m") com("n") com3(minspeed)
com("m") com("d") com3(midspeed)
com("m") com("a") com3(maxspeed)
end if
else
beep2 beep1 beep2 beep
midspeed = maxspeed / 2 + minspeed / 2
if noserial == 0 & debugspeed == 1 & noremote == high then
com("s") com("x") com3(speed)
end if
end if

speed = 0
tacho = 0
ddir = stop
setspeed

bumpersoff
if noserial == 0 & noremote == high then
com("b") com("u") com("m") com("p") com("e") com("r") com("s")
end if


photo -- read photo resistors
photo_init -- set comparator
if noserial == 0 & noremote == high then
com("p") com("h") com("o") com("t") com("o")
com("p") com("h") com("l") com3(phleft)
com("r") com3(phright)
end if
beep2
if noremote == high then
waitgp2d12 -- wave to me :-)
-- this checks gp2d12
else
bothposi(posm)
delay_1s(6)

f877_serial_setup ( 192 )
com("s") com("t") com("u") com("c") com("k") com(" ")
com("b") com("o") com("o") com("t") com(" ")
com("o") com("k") com(13) com(10)
t = 0
t2 = 0
beep2
delay_100ms(5)
beep3

while serin != "x" & dist < dstop loop
gp2d12
t = 0
while (t < 250 & dist < dmax) loop
serin = comin
if (serin == "x") then
t = 250
else
if (serin > 0) then
com(serin)
end if
end if
t = t + 1
delay_2ms
end loop
com(":") com(13) com(10);
end loop
end if
if serin == "x" then
noremote = low
-- dmode = dmodecontrolled
else
noremote = high
dmode = dmodefree
end if
beep3
delay_100ms(5)
beep2
delay_100ms(5)
beep2
-- ---------------------------------------------------------------------------
-- main loop
-- ---------------------------------------------------------------------------
forever loop
if mainwait > 0 then
delay_5ms(mainwait)
end if
if noserial == 0 then show_values("M","A","I") end if
set -- check all the sensor values
-- set driving parameters

if noserial == 0 & maindebug > 0 then
show_values("m","a","s")
end if
checkbumpers -- contact?
if speed > 0 then
setsteer -- set steering
end if
driverc -- calculate speed and drive
checkbumpers -- contact?
photo -- check photo resistors

if ddir == stop & speed == 0 then
setspeed
-- stopped - what next?
-- global loop - dont drive forever
if noremote == high then
gloop = gloop - 1
if gloop == 0 then
waitgp2d12
end if
initstart
setstart
else
show_stuck
initstart
if noserial == 0 then
com(".") com(13) com(10)
end if
serin = comwait
if serin != "." then
commands
else
if noserial == 0 then
com(".") com(13) com(10)
end if
end if
end if
else
if noremote == low then
serin = comin
if serin > 0 then
commands
beep
end if
end if
end if
if ddir == forward then
checkbumpers -- contact?
end if

-- debug info
if noserial == 0 & maindebug > 0 then
show_values("m","a","i")
end if
testled = off
if alldebug == 0 then
-- no serial info out - give things some more time to happen
-- delay_10ms
end if
end loop

-- End of program.