include f877_20
include jlib
include f877_modules
include const
include upins
include var
include steer
include polaroid

procedure show_stuck is
if noserial == 0 & stuck > stuckinit then
if stuck == stuckgp2d12 then
com("s") com("t") com("u") com("c") com("k") com(" ") com("g") com("p") com("2") com("d") com("1") com("2")
com(13) com(10)
elsif stuck == stuckpolaroid then
com("s") com("t") com("u") com("c") com("k") com(" ") com("p") com("o") com("l") com("a") com("r") com("o") com("i") com("d")
com(13) com(10)
elsif stuck == stuckrev then
com("s") com("t") com("u") com("c") com("k") com(" ") com("r") com("e") com("v") com("e") com("r") com("s") com("e")
com(13) com(10)
elsif stuck == stuckblock then
com("s") com("t") com("u") com("c") com("k") com(" ") com("b") com("l") com("o") com("c") com("k") com("e") com("d")
com(13) com(10)
elsif stuck == stuckbumperl then
com("s") com("t") com("u") com("c") com("k") com(" ") com("b") com("u") com("m") com("p") com("e") com("r") com(" ") com("l")
com(13) com(10)
elsif stuck == stuckbumperr then
com("s") com("t") com("u") com("c") com("k") com(" ") com("b") com("u") com("m") com("p") com("e") com("r") com(" ") com("r")
com(13) com(10)
elsif stuck == stucktacho then
com("s") com("t") com("u") com("c") com("k") com(" ") com("t") com("a") com("c") com("h") com("o")
com(13) com(10)
end if
stuck = stuckinit
end if
end procedure

procedure show_values ( byte in v1, byte in v2, byte in v3) is
if doshowvalues != 0 then
com(13) com(10)
com(v1) com(v2) com(v3) com(" ")
if ddir != a_ddir then
if fulldebug == 1 then
com("d") com("d") com("i") com("r") com(" ")
else
com("d") com("d") com(" ")
end if
com(ddir + "0" ) com(" ")
end if
if drivecount != a_drivecount then
if fulldebug == 1 then
com("d") com("c") com("o") com("u") com("n") com("t") com(" ")
else
com("d") com("c") com(" ")
end if
com3(drivecount)
end if
if tacho != a_tacho then
if fulldebug == 1 then
com("t") com("a") com("c") com("h") com("o") com(" ")
else
com("t") com("a") com(" ")
end if
com3(tacho)
end if
if cny70 != a_cny70 then
if fulldebug == 1 then
com("c") com("n") com("y") com(" ")
else
com("c") com("n") com(" ")
end if
com3(cny70)
end if
if stuckcounter != a_stuckcounter then
if fulldebug == 1 then
com("s") com("t") com("u") com("c") com("k") com(" ")
else
com("s") com("t") com(" ")
end if
com3(stuckcounter)
end if
if stuckcounter2 != a_stuckcounter2 then
if fulldebug == 1 then
com("s") com("t") com("2") com("c") com("k") com(" ")
else
com("s") com("2") com(" ")
end if
com3(stuckcounter2)
end if
-- if steer != a_steer then
if fulldebug == 1 then
com("d") com("d") com("i") com("r") com("r")
else
com("d") com("d")
end if
com(ddir+"0") com(" ") com3(speed)
if fulldebug == 1 then
com("s") com("t") com("e") com("e") com("r")
else
com("s") com("t") com("e") com("r")
end if
com(" ") com3(steer) -- com3(steercount)
-- end if
if ppoint != a_ppoint then
if fulldebug == 1 then
com("p") com("p") com("o") com("i") com("n") com("t")
else
com("p") com("p") com("n") com("t")
end if
com(" ") com3(ppoint)
end if
if nogp2d12 == 0 then
if dist != a_dist then
if fulldebug == 1 then
com("d") com("i") com("s") com("t") com(" ")
else
com("d") com("i") com(" ")
end if
com3(dist)
end if
end if
if pdist != a_pdist then
if fulldebug == 1 then
com("p") com("d") com("i") com("s") com("t") com(" ")
else
com("p") com("d") com(" ")
end if
com3(pdist)
end if
if phleft != a_phleft then
if fulldebug == 1 then
com("l") com("l") com("e") com("f") com("t") com(" ")
else
com("l") com("l") com(" ")
end if
com3(phleft)
end if
if phright != a_phright then
if fulldebug == 1 then
com("l") com("r") com("i") com("g") com("h") com(" ")
else
com("l") com("r") com(" ")
end if
com3(phright)
end if
else
if steer != a_steer | ddir != a_ddir then
com(13) com(10)
com(v1) com(v2) com(v3) com(" ")
com("s") com("t") com("e") com("e") com("r")
com(" ") com3(steer)
com("d") com("d") com("i") com("r") com("r")
com(ddir+"0") com(" ") com3(speed)
end if
end if
a_speed = speed
a_ddir = ddir
a_drivecount = drivecount
a_tacho = tacho
a_cny70 = cny70
a_stuckcounter = stuckcounter
a_stuckcounter2 = stuckcounter2
a_steer = steer
a_ppoint = ppoint
a_dist = dist
a_pdist = pdist
a_phleft = phleft
a_phright = phright
end procedure

procedure initstart is
a_speed = 0
a_ddir = 0
a_drivecount = 0
a_tacho = 0
a_cny70 = 0
a_stuckcounter = 0
a_stuckcounter2 = 0
a_steer = 0
a_ppoint = 0
if nogp2d12 == 0 then
a_dist = 0
end if
a_pdist = 0

cny70 = 0
ddir = stop
drivecount = 0
foundm = 0 foundl = 0 foundr = 0
foundll = 0 foundrr = 0
speed = 0
steer = smiddle
stuckcounter2 = 0
stuckcounter = 0
tacho = 0
end procedure

procedure bumpersoff is
-- check bumpers before driving off

bank_1
F877_ADCON1 = 0b_1000_0111 -- all digital IO
bank_0

while (bumperl == low) | (bumperr == low) loop
if bumperl == low then
if noserial == 0 then
com("b") com("u") com("m") com("p") com("e") com("r") com("l")
end if
testled = on
delay_100ms
testled = off
delay_100ms
end if
if bumperr == low then
if noserial == 0 then
com("b") com("u") com("m") com("p") com("e") com("r") com("r")
end if
breakled = on
delay_100ms
breakled = off
delay_100ms
end if
end loop
end procedure

procedure bothservo is
-- beware - draws a bit of current!
var byte npoint = posn - ppoint
if nopolaroid == 0 then
if nogp2d12 == 0 then
vpulse = high
delay_10us( ppoint )
vpulse = low
ppulse = high
delay_10us( npoint )
ppulse = low
else
vpulse = high
delay_10us( ppoint )
vpulse = low
end if
end if
end procedure


procedure polservo (byte in polpoint) is
t = ppoint
while t > polpoint loop
vpulse = high
delay_10us( t )
vpulse = low
t = t - posstep
delay_5ms(3)
end loop
while t < polpoint loop
vpulse = high
delay_10us( t )
vpulse = low
t = t + posstep
delay_5ms(3)
end loop
ppoint = t
end procedure


procedure bothposi (byte in polpoint) is
-- polpoint = target position
-- ppoint = current position

while polpoint < ppoint loop
ppoint = ppoint - 1
bothservo
delay_5ms(3)
end loop


while polpoint > ppoint loop
ppoint = ppoint + 1
bothservo
delay_5ms(3)
end loop

end procedure


procedure pspoint is
if dopoint == 1 & ddir != stop then
if pdir == high then
if ppoint < posm + pforward / 2 then
ppoint = ppoint + posstep
else
pdir = low
end if
else
if ppoint > posm - pforward / 2 then
ppoint = ppoint - posstep
else
pdir = high
end if
end if
if nopsp == 0 then
bothservo
end if
end if
end procedure

procedure lightsteer is
if debuglight != 0 then
com("l") com("i") com3(steer)
end if
if phleft > 0 & phright > 0 then
-- set direction according to photo resistors
if steer > shright & steer < shleft then
-- we're going about straight
if phright / 2 + 128 - phleft / 2 > phlevel then
steer = steer - 1
else
steer = steer + 1
end if
end if
end if
end procedure

procedure lightdir is
if phleft > 0 & phright > 0 then
-- set direction according to photo resistors
if phright / 2 + 128 - phleft / 2 > phlevel then
steer = shleft
else
steer = shright
end if
end if
if debuglight == 1 & noserial == 0 then
show_values("l","d","i")
end if
end procedure

procedure adc1 is begin
-- adc routine with no parameters
if( noad == 0 ) then
f877_ADCON0 = 0 -- Stop ADC
bank_1
f877_ADCON1 = ( 9 & 0x0F ) -- PCFG bits and left justified
-- f877_ADCON1 = ( 9my_PCFG & 0x0F ) -- PCFG bits and left justified
f877_ADFM = low
STATUS_C = low
bank_0
if target_clock == 4_000_000 then
f877_ADCON0 = 0x40 | ( ( t & 0x07 ) << 3 ) -- channel and clock
end if
if ( target_clock == 10_000_000 ) | ( target_clock == 20_000_000 ) then
f877_ADCON0 = 0x80 | ( ( t & 0x07 ) << 3 ) -- channel and clock
end if
f877_ADCON = high -- turn on ADC module
delay_10uS( 3 ) -- wait Tacq
f877_ADCGO = high -- Start conversion
while f877_ADCGO loop
end loop
f877_ADCON0 = 0 -- Stop ADC
t = f877_ADRESH
end if
end procedure


procedure adc is begin
-- adc routine with no parameters
-- ad channel in var byte channel, result in var byte ad
if( noad == 0 ) then
f877_ADCON0 = 0 -- Stop ADC
bank_1
f877_ADCON1 = ( 0x0E & 0x0F ) -- PCFG bits and left justified
-- f877_ADCON1 = ( 9 & 0x0F ) -- PCFG bits and left justified
-- f877_ADCON1 = ( 9my_PCFG & 0x0F ) -- PCFG bits and left justified
f877_ADFM = low
STATUS_C = low
bank_0
if target_clock == 4_000_000 then
f877_ADCON0 = 0x40 | ( ( t & 0x07 ) << 3 ) -- channel and clock
end if
if ( target_clock == 10_000_000 ) | ( target_clock == 20_000_000 ) then
f877_ADCON0 = 0x80 | ( ( t & 0x07 ) << 3 ) -- channel and clock
end if
f877_ADCON = high -- turn on ADC module
delay_10uS( 3 ) -- wait Tacq
f877_ADCGO = high -- Start conversion
while f877_ADCGO loop
end loop
f877_ADCON0 = 0 -- Stop ADC
t = f877_ADRESH
end if
end procedure



procedure gp2d12 is
-- read SHARP gp2d12 sensor
t= gp2d12channel
adc1
if t > 126 then
t = 126 end if
-- dist = ( dist + t ) / 2
dist = t
if noserial == 0 & debuggp2d12 == 1 then
com("D") com3(dist)
end if
if ppoint == posl then
if debugsenval == 1 then com("i") com("l") com3(dist) end if
foundrr = dist
elsif ppoint == poshl then
if debugsenval == 1 then com("i") com("h") com3(dist) end if
foundr = dist
elsif ppoint == posm then
if debugsenval == 1 then com("i") com("m") com3(dist) end if
foundm = dist
elsif ppoint == poshr then
if debugsenval == 1 then com("i") com("h") com3(dist) end if
foundl = dist
elsif ppoint == posr then
if debugsenval == 1 then com("i") com("r") com3(dist) end if
foundll = dist
end if
end procedure

procedure init is
if noserial == 0 then
com("i") com("n") com("i") com("t")
end if
testled = on
steer = sleft
ppoint = posm
for 10 loop
bothposi(posl)
setsteer
delay_100ms(1)
end loop
if noserial == 0 then
com3(ppoint) com3(steer) com(13) com(10)
end if
testled = off
ppoint = posm
steer = smiddle
testled = on
for 10 loop
bothposi(posm)
setsteer
delay_100ms(1)
end loop
if noserial == 0 then
com3(ppoint) com3(steer) com(13) com(10)
end if
testled = on
testled = off
ppoint = posr
steer = sright
testled = on
for 10 loop
bothposi(posr)
setsteer
delay_100ms(1)
end loop
if noserial == 0 then
com3(ppoint) com3(steer) com(13) com(10)
end if
testled = off
testled = off
ppoint = posm
steer = smiddle
testled = on
for 10 loop
bothposi(posm)
setsteer
delay_100ms(1)
end loop
if noserial == 0 then
com3(ppoint) com3(steer) com(13) com(10)
end if
end procedure




-- --------------------------------------------------------------------
-- Sound
-- --------------------------------------------------------------------

procedure frequency ( byte in period, byte in prescale ) is
if nosound == 0 then

-- only beep when drive pwm is off, shared PR2
if f877_CCP1CON == 0 then
if period > 0 then
asm movf period, w
bank_1
asm movwf F877_PR2
bank_0
F877_CCPR2L = period / 2 -- set duty cycle at approx 0.5
pin_c1_direction = output

if prescale == 16 then
F877_T2CON = 0b_0000_0110 -- tmr2 on, prescale=16
elsif prescale == 4 then
F877_T2CON = 0b_0000_0101 -- tmr2 on, prescale=4
elsif prescale == 1 then
F877_T2CON = 0b_0000_0100 -- tmr2 on, prescale=1
end if
F877_CCP2CON = 0b_0000_1100 -- pwm mode on
else
F877_CCP2CON = 0 -- pwm mode off
end if
else
F877_CCP2CON = 0 -- pwm mode off
end if
end if
end procedure


procedure beep is
frequency(70, 16)
delay_50ms( 1 )
frequency(170, 16)
delay_50ms( 1 )
frequency(0, 16)
end procedure

procedure beep2 is
t = 0
for 50 loop
t = t + 1
frequency(50 + t, 16)
delay_1ms( 3 )
end loop
frequency(0,16)

end procedure

procedure beep1 is
t = 50
for 50 loop
t = t - 1
frequency(50 + t, 16)
delay_1ms( 3 )
end loop
frequency(0,16)

end procedure

procedure beep3 is
t = 50
for 50 loop
t = t - 1
frequency(50 + t, 16)
delay_1ms( 3 )
frequency(t, 16)
delay_1ms( 3 )
end loop
frequency(0,16)


end procedure

-- --------------------------------------------------------------------
-- Photo resistor code
-- --------------------------------------------------------------------

procedure photo is
-- read photoresistors

t = photolchannel
adc
phleft = t
t = photorchannel
adc
phright = t
end procedure

procedure photo_init is
phlevel = phright / 2 + 128 - phleft / 2
if nosound == 0 then
frequency(phleft,16)
delay_50ms(5)
frequency(phright,16)
delay_50ms(5)
frequency(0,16)
end if
end procedure

-- --------------------------------------------------------------------
-- Speed handling
-- --------------------------------------------------------------------
procedure setspeed is begin
myspeed = speed
if midspeed > minspeed then
-- only when speed are initialized
-- if myspeed <= midspeed & ddir > stop & tacho < 1 then
-- -- give it a kick when it starts at low speed
-- if noserial == 0 & debugspeed != 0 then com("k") end if
-- myspeed = 2 * minspeed
-- end if
-- if cny70 < cny70max & tacho == 0 & ddir > stop then
-- end if
end if
-- test speed and not myspeed to allow a quickstop
if speed == 0 then
f877_CCP1CON = 0 -- Stop PWM
pin_c2_direction = input -- Set PortC:2 as input
else
if f877_CCP1CON == 0 then -- Check if already running
f877_PR2 = 30 -- PWM period
f877_CCPR1L = myspeed -- Duty cycle
pin_c2_direction = output -- Set PortC:2 as output
pin_c2 = low
f877_T2CON = 0x06 -- TRM2 prescaler=1 postscaler=16 Timer2 ON
f877_CCP1CON = 0x0C -- PWM mode and simetric duty cycle
else
f877_CCPR1L = myspeed -- Duty cycle. Updated in next PWM period
end if
end if
end procedure


procedure driverc is
var bit drivet
drivet = low


if ( debugspeed == 1 & noserial == 0 ) then
show_values("d","c","b")
end if

if ddir == forward then
-- set direction
mr = low
mf = high
drivet = high
end if

if ddir == stop then
breakled = on
end if

if ddir == reverse then
-- set direction
mf = low
mr = high
drivet = high
end if

if drivet == low then
-- no more driving - slow down quickly
if speed >= minspeed - 3 * speedstep then
speed = speed - speedstep
else
-- come to a complete stop
speed = 0
tacho = 0
mf = low
mr = low
cny70 = 0
drivecount = driveconst / 2
if noserial == 0 & noremote == high then com("y") com("3") end if
end if
else
breakled = off -- we're driving
end if


if nopwm == 0 then
setspeed
else
frequency(speed, 16)
end if

if debugspeed == 1 & noserial == 0 then
show_values("d","c","e")
end if
end procedure


procedure checkbumpers is
-- bumpers are pulled up

bank_1
F877_ADCON1 = 0b_1000_0111 -- all digital IO
bank_0
if (bumperl == low & bumperr == low) then
mf = low mr = low
speed = 0
ddir = stop
-- if dmode == dmodefree then
-- ddir = reverse
-- steer = smiddle
-- speed = minspeed
-- end if
cny70 = 0
tacho = 0
drivecount = reverseconst
-- beep3
stuck = stuckbumperl
else
if bumperl == low then
if ddir == reverse then
else
mf = low mr = low
breakled = on
drivecount = reverseconst
cny70 = 0
tacho = 0
speed = 0
ddir = stop
-- if dmode == dmodefree then
-- ddir = reverse
-- steer = sleft
-- speed = minspeed
-- setsteer
-- end if
stuck = stuckbumperl
if noserial == 0 then
com("b") com("p") com("l")
end if
beep1
end if
end if
if bumperr == low then
if ddir == reverse then
else
mf = low mr = low
breakled = on
ddir = stop
cny70 = 0
tacho = 0
-- if dmode == dmodefree then
-- ddir = reverse
-- speed = minspeed
-- steer = sright
-- setsteer
-- end if
stuck = stuckbumperr
drivecount = reverseconst
if noserial == 0 then
com("b") com("p") com("l")
end if
-- beep2
end if
end if
end if
end procedure