'------------------------------------------------------
'  Name     : 819test.bas                        
'  Compiler : PicBasic Pro - MicroEngineering Labs
'  Notes    : Setting internal oscillator on 16F819                       
'------------------------------------------------------

' PortA set as outputs.
trisa = %00000000

' PortB set as outputs.  
trisb = %00000000   

'------------------------------------------------------
' initialize variables

trigger          VAR PORTA.0
echo             VAR PORTA.1
piezo            VAR PORTA.3
switch           VAR PORTB.0
enable_right     VAR PORTB.1
forward_right    VAR PORTB.2
reverse_right    VAR PORTB.3
enable_left      VAR PORTB.4
reverse_left     VAR PORTB.5
forward_left     VAR PORTB.6
dist_raw         VAR WORD
dist_inch        VAR WORD
conv_inch        CON 15
I                VAR BYTE
temp             VAR BYTE
state            VAR BYTE
best_pos         VAR BYTE
most_space       VAR BYTE
position         VAR WORD[12]

low enable_left
low forward_left
low reverse_left

low enable_right
low forward_right
low reverse_right

SOUND PIEZO,[115,10,50,10]

start:

' rotate robot to the left

For I = 1 to 5        
   state = switch
   while switch = state
      gosub turn_left
   wend
Next I

position[11] = 0

' take 11 distance measurements and store the
' results in the distance[11] array

For I = 0 to 10
   gosub sr_sonar
   position[I] = dist_inch
   state = switch
   while switch = state
      gosub turn_right
   wend
Next I

' sort the distance array to find the location
' with the most free space 

best_pos = 11

For I = 0 to 10 
   If position[I] >= position[best_pos] then
      best_pos = I
   Endif     
Next I

most_space = 11 - best_pos

' rotate the robot so that it is pointing towards 
' the are with the most free space

For I = 1 to most_space
   state = switch
   while switch = state
      gosub turn_left
   wend
Next I

' Move the robot forward into the area that was
' determined to be the most free of obstacles
' check for any obstacles while moving forward
' move in reverse and then scan for area with 
' most space if an obstacle was encountered

For I = 1 to 24
   gosub sr_sonar
   If dist_inch < 8 then
      SOUND PIEZO,[115,5,90,2,80,4,50,10]
      For temp = 1 to 6
         state = switch
         while switch = state
         gosub backwards
         wend
      Next temp
      goto start
   Endif
   
   state = switch
   while switch = state
      gosub forward
   wend
   
Next I 

goto start

end

'------------------------------------------------------
' movement subroutines

forward:

   high enable_left
   high forward_left   
   high enable_right
   high forward_right
   
   pause 20
   
   low enable_left
   low forward_left   
   low enable_right
   low forward_right      

   pause 20
   
return

'------------------------------------------------------

turn_left:

   high enable_left
   high forward_left
   high enable_right
   high reverse_right

   pause 5
   
   low enable_left
   low forward_left
   low enable_right
   low reverse_right
   
   pause 5
   
return

'------------------------------------------------------

backwards:

   high enable_left
   high reverse_left
   high enable_right
   high reverse_right
   
   pause 20

   low enable_left
   low reverse_left
   low enable_right
   low reverse_right

   pause 10
   
return

'------------------------------------------------------

turn_right:

   high enable_left
   high reverse_left
   high enable_right
   high forward_right
   
   pause 5
   
   low enable_left
   low reverse_left
   low enable_right
   low forward_right
   
   pause 5
   
return

'------------------------------------------------------

sr_sonar:

   pulsout trigger,1			
   pulsin echo,1,dist_raw
   dist_inch = (dist_raw/conv_inch) 
   pause 2

return

end