'*********************************************************************************
' room-mapping.bas
' The robot will take 11 distance readings from the SRF04 covering 180
' degrees. The robot will then move to the area with the greatest 
' amount of free space.            
' PicBasic Pro Compiler 
'*********************************************************************************

trisa = %00000000                   		' set porta to outputs
trisb = %00001000                   		' set portb pin 3 to input

m_servo  	var 	byte        	' define variables and constants
l_servo  	var 	byte
r_servo  	var 	byte
timer    	var 	byte                  
right_led    	var 	PORTB.0
left_led   	var 	PORTB.1
trigger     	var 	PORTB.2
echo        	var 	PORTB.3	
piezo       	var 	PORTB.4
right_servo    	var	PORTB.5
left_servo      var	PORTB.6
mid_servo       var	PORTB.7
dist_raw    	var	word
dist_inch   	var 	word
conv_inch   	con	15             	' conversion factor for inches
turn            var	byte
I		var	byte
best_pos	var	byte
position	var	word[12]
most_space      var	byte

low trigger				' set trigger pin to logic 0
low left_led				' turn off left LED
low right_led				' turn off right LED

main:

     for turn = 1 to 5                       		' rotate robot 90 degrees to the left
         gosub turn_left
     next turn

     position[11] = 0                        		' set last position of array to 0

     for i = 0 to 10                         		' take 11 distance measurements
         gosub sr_sonar			' get sonar reading
         position[i] = dist_inch            		' store sonar reading in the array
         gosub turn_right                   		' rotate the robot 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		'of the largest distance measurement
               best_pos = i
         endif
     next i

     most_space = 11 - best_pos                          ' number of left rotations needed to get the
                                                       ' robot back to position with the most space
     for turn = 1 to most_space              		' postion robot in the direction with the 
         gosub turn_left                     			' most free space.
     next turn

     for turn = 1 to 15                     			' walk forward for 15 steps
         gosub sr_sonar                      			' take sonar reading
         if dist_inch < 6 then               			' if an object is detected then
            gosub walk_reverse               		' back the robot up a little
            goto main                        			' scan for area with the most space again
         endif
         Gosub walk_forward
     next turn

goto main

end 

walk_forward:                           					' walk froward subroutine
		m_servo = 170
		gosub servo
		l_servo = 160
		r_servo = 160
		gosub servo
		m_servo = 100
		gosub servo
		l_servo = 120
		r_servo = 120
		gosub servo
return

walk_reverse:                            					' walk reverse subroutine
             for turn = 0 to 3           
		sound portb.4,[90,1,80,2,125,1,90,2,100,2]
		m_servo = 170
		gosub servo
		l_servo = 120
		r_servo = 120
		gosub servo
		m_servo = 100
		gosub servo
		l_servo = 160
		r_servo = 160
		gosub servo
             next turn
return

turn_left:                                 				 ' rotate the robot left 15 degrees
             sound portb.4,[140,1,80,2,125,1,95,2]
                 m_servo = 170
                 gosub servo
                 l_servo = 120
                 r_servo = 160
                 gosub servo
                 m_servo = 100
                 gosub servo
                 l_servo = 160
                 r_servo = 120
                 gosub servo
return

turn_right:                                  				' rotate the robot right 15 degrees
             sound portb.4,[140,1,120,2,110,1,100,2]
                 m_servo = 170
                 gosub servo
                 l_servo = 160
                 r_servo = 120
                 gosub servo
                 m_servo = 100
                 gosub servo
                 l_servo = 120
                 r_servo = 160
                 gosub servo
return

sr_sonar:

         pulsout trigger,1				' send a 10us trigger pulse to the SRF04
         pulsin echo,1,dist_raw			' start timing the pulse width on echo pin 
         dist_inch = (dist_raw/conv_inch)		' Convert raw data into inches 
         pause 1					' wait for 10us before returning to main

return

servo:                           				' subroutine to set servos
       for timer = 1 to 10
       pulsout mid_servo,m_servo
       pulsout left_servo,l_servo
       pulsout right_servo,r_servo
       pause 13
       next timer
return