init:
servo r,r_off
servo l,l_off
servo hood,closehood
servo pan,cen
pause 30
HI2cSetup i2cmaster, %11100000, i2cslow, i2cbyte
let b3 = 10 'send cmd to cylon
HI2cOut 0,( b3 )
high g_led
pause 100
main:
low g_led
high b_led
pause 100
do
pause 100
if irflag = 1 then gosub getdata
select case cmnd
case 1
gosub roam
case 2
gosub closehod
case 3
gosub bck
case 4
gosub openhod
case 5
gosub rgtturn
case 6
gosub allstop
case 7
gosub lftturn
case 8
gosub openhod
case 9
gosub fwd
' case 10
' gosub scanlft
'case 22
'gosub t1iput
end select
loop
getdata:
pulsout toir,5
serin fromir,n2400_8,cmnd
sertxd(#cmnd,cr,lf)
return
'#### luna's drive subroutines ##############
fwd:
' sertxd(" forward ",cr,lf)
high r_led
low g_led
low b_led
pause 100
servopos r, r_forward_slow
servopos l, l_forward_slow
return
bck:
'sertxd(" backwards ",cr,lf)
low r_led
high b_led
low g_led
pause 100
servopos r, r_back_slow
servopos l, l_back_slow
return
allstop:
' sertxd(" allstop ",cr,lf)
low b_led
low r_led
low g_led
servopos r, r_off
servopos l, l_off
return
rgtturn:
' sertxd("rightturn",cr,lf)
low b_led
high g_led
pause 10
servopos l,l_forward_slow
servopos r,r_back_slow
return
lftturn:
' sertxd(" leftturn ",cr,lf)
low b_led
high g_led
servopos l,l_back_slow
servopos r,r_forward_slow
return
lft90:
gosub allstop
'servopos l,l_back_slow
'servopos r,r_back_slow
pause 30
servopos l,l_back_slow
servopos r,r_forward_slow
pause 1300
gosub allstop
return
lft180:
gosub allstop
'servopos l,l_back_slow
'servopos r,r_back_slow
pause 30
servopos l,l_back_slow
servopos r,r_forward_slow
pause 2500
gosub allstop
return
rgt90:
gosub allstop
pause 30
'servopos l,l_back_slow
'servopos r,r_back_slow
servopos l,l_forward_slow
servopos r,r_back_slow
pause 1300
gosub allstop
pause 30
return
rgt180:
gosub allstop
'servopos l,l_back_slow
'servopos r,r_back_slow
pause 30
servopos r,r_back_slow
servopos l,l_forward_slow
pause 2500
gosub allstop
pause 30
return
'###### luna hood subroutines##########
openhod:
pause 10
gosub cylonl
pause 10
for b6 =closehood to openhood step -2
servopos hood,b6
pause 200
next b6
let cmnd = 0
return
closehod:
pause 10
gosub split1
pause 10
for b9 =openhood to closehood step 2
servopos hood,b9
pause 200
next b9
let cmnd = 0
return
roam:
high b_led
pause 10
gosub openhod
for counter = 1 to 3
servopos r, r_forward_slow
servopos l, l_forward_slow
for b4 = lft to rgt step 2
servo pan,b4
pulsout ping,4 ' produce 20uS trigger pulse (must be minimum of 10uS)
pulsin echo,1,range : range = range * 2 ' measures the range in 10uS steps
' recharge period after ranging completes
let range=range*10/ 115
if range < 25 then gosub rgt90
if range < 15 then gosub lft180
pause 30
next b4
for b4 = rgt to lft step - 2
servopos r, r_forward_slow
servopos l, l_forward_slow
servo pan,b4
pulsout ping,4 ' produce 20uS trigger pulse (must be minimum of 10uS)
pulsin echo,1,range : range = range * 2 ' measures the range in 10uS steps
' recharge period after ranging completes
let range=range*10/ 115
if range < 25 then gosub lft90
if range < 15 then gosub rgt180
pause 30
next b4
next counter
servopos pan,cen
pause 30
gosub allstop
pause 30
gosub closehod
pause 30
return
'######### I2C camds ##################################
' following subrotines using i2c setup sends the values b3 hi2cout slot 0
' the 20x2 led module receives the cammand is sent three times
cylonl:
for counter = 1 to 3
let b3 = 10 'send cmd to cylon
HI2cOut 0,( b3 )
sertxd("sent cylon ",#b3,13,10)
next counter
return
split1:
for counter = 1 to 3
let b3 = 30 'send cmd to cylon
HI2cOut 0,( b3 )
sertxd("sent split ",#b3,13,10)
next counter
return