hello,
this is a peace of my code, the main routine called main is detecting object and avoiding them and the interrupt subroutine is called start.
basically its a robot which detect and avoid obstacle (which works fine ) but should jump to a subroutine if either port.b0 or port.b1 or portb2 or portb3 goes low (meaning that an edge is detected) in order to avoid the edge ;and as soon as the 4 portbs '0 to 3' are high together (meaning that no edge is detected) the program should go back to the main routine.
here is the code:
'-------------------------interrupt set up ----------------------
ON INTERRUPT GOTO start
resume
'------------------------obstacle mode-----------------------------
main:
INTCON = %10010000 'enable interrupt on portb.0
gosub turnhead_extleft 'position head to the left
position[11] = 0 'reset last position of array to 0
for i = 0 to 10 'take 11 distance measurements
gosub sr_sonar 'get sonar readings for each of the 11 dist measuremens
position[i] = dist_inch 'store sonar reading in the array
gosub turnhead_right 'rotate head right by 15 degrees
next i 'do it again
best_pos = 11 ' ?????
for i = 0 to 10 'sorting routine to find location
if position[i] >= position[best_pos] then 'if the distance measured [i] is > than 11 inches
best_pos = i 'then best position will be that distance i
serout lcd,baud,[254,135,#best_pos," "]
endif
next i
most_space = 11 - best_pos 'number of left positions needed to get the bot head
'back to position with the most space
for turn = 1 to most_space 'position the bot head in the direction with the most
gosub turnhead_left 'free space
next turn
for turn = 1 to 15 'walk forward for 15 steps
gosub sr_sonar 'take sonar readings
if dist_inch < 6 then 'if obect is too close,ie <6inches
gosub walk_reverse 'back the bot up for 10 steps
goto main 'go back to start and scan for the most space again
endif
gosub walk_forward
next turn
goto main
end
'--------------------end obstacle mode------------------------------------------
'--------------------edge mode-------------------------------------------------
start:
if portb.0 = 0 && portb.1 = 0 && portb.2 = 0 && portb.3 = 0 then 'if nothing is detected
goto do_nothing
endif
if portb.0 = 0 && portb.1 = 0 && portb.2 = 0 && portb.3 = 1 then 'if left is detected
goto stop_to_move
endif
if portb.0 = 0 && portb.1 = 0 && portb.2 = 1 && portb.3 = 0 then 'if right is detected
goto stop_to_move
endif
if portb.0 = 0 && portb.1 = 0 && portb.2 = 1 && portb.3 = 1 then 'if right and left are detected
goto stop_to_move
endif
if portb.0 = 0 && portb.1 = 1 && portb.2 = 0 && portb.3 = 0 then 'if back is detected
goto back
endif
if portb.0 = 0 && portb.1 = 1 && portb.2 = 0 && portb.3 = 1 then 'if back and left are detected
goto back_headright
endif
if portb.0 = 0 && portb.1 = 1 && portb.2 = 1 && portb.3 = 0 then
goto back_headleft
endif
if portb.0 = 0 && portb.1 = 1 && portb.2 = 1 && portb.3 = 1 then
goto slow_back_headextright
endif
if portb.0 = 1 && portb.1 = 0 && portb.2 = 0 && portb.3 = 0 then
goto front
endif
if portb.0 = 1 && portb.1 = 0 && portb.2 = 0 && portb.3 = 1 then
goto front_left
endif
if portb.0 = 1 && portb.1 = 0 && portb.2 = 1 && portb.3 = 0 then
goto front_right
endif
if portb.0 = 1 && portb.1 = 0 && portb.2 = 1 && portb.3 = 1 then
goto front
endif
if portb.0 = 1 && portb.1 = 1 && portb.2 = 0 && portb.3 = 0 then
goto front
endif
if portb.0 = 1 && portb.1 = 1 && portb.2 = 0 && portb.3 = 1 then
goto front_left
endif
if portb.0 = 1 && portb.1 = 1 && portb.2 = 1 && portb.3 = 0 then
goto front_right
endif
if portb.0 = 1 && portb.1 = 1 && portb.2 = 1 && portb.3 = 1 then
goto front
endif
goto start





Bookmarks