‘CMU tracking program
‘By J. Iovine
define osc 16
recdata var byte[10]
x var byte
confid var byte
trisb = 0
portb = 0
pause 1500
serout port.1,6,[“CMU Prg.”]
serout portb.2,2,[“RS”, 13]
pause 1250
‘Reset CMU camera
serout portb.2,2,[“RS”, 13]
262 Chapter Fourteen
gosub display
‘Turn green CMU LED on
serout portb.2,2,[“L1 1”,13]
gosub display
portb.3 = 1
‘Turn on auto lighting adjust & fluorescent band filter ***
serout portb.2,2,[“CR 45 7 18 44”, 13]
gosub display
serout portb.1,6,[“A L”] ‘Auto lighting adjustment
pause 20000 ‘Hold 20 seconds
‘Turn off auto lighting adjust
serout portb.2,2,[“CR 18 44 19 32”, 13]
gosub display
‘Turn green CMU LED off
serout portb.2,2,[“L1 2”,13]
gosub display
portb.3 = 0
‘Set poll mode1
packet
serout portb.2,2,[“PM 1”, 13]
pause 100
‘Set raw data
serout portb.2,2,[“RM 3”, 13]
for x = 0 to 10 ‘Blink red LED to tell user to ready target
portb.3 = 1
pause 250
portb.3 = 0
pause 250
next x
portb.6 = 1 ‘Track LED on
‘Track window command looks at center of CMU window
‘Grabs data and sends it to track color function
‘Track:
serout portb.2,2,[“TW”, 13]
pause 2000
portb.6 = 0 ‘Track LED off
main:
Color Robotic Vision System 263
portb.3 = 1
‘Send commandtrack
color (with no arguments)
‘Will track last color grabbed by TW command
serout portb.2,2,[“TC”, 13]
‘Gather the m statistics packet from TW command
serin2 portb.0,84,[str recdata\10]
confid = recdata[9]
if recdata[2] > 50 and confid > 20 then left ‘MMX
if recdata[2] < 40 and confid > 20 then right ‘MMX
if recdata[8] < 175 and confid > 25 then fwd ‘PIX
if recdata[8] > 200 and confid > 25 then bwd ‘PIX
serout portb.1,6,[254,1,“S”] ‘Stop
portb.3 = 0
pulsout portb.4, 668 ‘Right servo stop
pulsout portb.5, 676 ‘Left servo stop
pause 18
portb.3 = 1
goto main
left:
serout portb.1,6,[254,1,“L”, #recdata[2]]
for x= 1 to 7
pulsout portb.4, 696 ‘Right servo forward
pulsout portb.5, 676 ‘Left servo stop
pause 20
next x
goto main:
right:
serout portb.1,6,[254,1,“R”,#recdata[2]]
for x= 1 to 7
pulsout portb.4, 668 ‘Right servo stop
pulsout portb.5, 648 ‘Left servo forward
pause 20
next x
goto main:
fwd:
serout portb.1,6,[254,1,“F”,#recdata[8]]
for x= 1 to 7
pulsout portb.4, 696 ‘Right servo forward
pulsout portb.5, 648 ‘Left servo forward
pause 20
next
264 Chapter Fourteen
goto main:
bwd:
serout portb.1,6,[254,1,“B”,#recdata[8]]
for x= 1 to 7
pulsout portb.4, 640 ‘Right servo backward
pulsout portb.5, 704 ‘Left servo backward
pause 20
next x
goto main:
display:
serin2 portb.0,84,20,main,[str recdata\3]
for x = 0 to 3
serout2 portb.1,16468,[“ ”,recdata[x]]
next x
pause 1500
serout2 portb.1,16468,[254,1]
return
