'********************************************************
' radar-walk.bas
' robot maintains a constant distance from an object
' if the object is within 15 to 20 centimeters then the
' robot will stand still. If an object is closer than 
' 15 cm then the robot will back away. If an object is 
' beyond 20 cm then the robot will move towards it.            
' 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                  
left_led    	var 	PORTB.0
right_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
dist_cm     	var 	word
conv_inch   	con 	15
conv_cm     	con 	6

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

main:

     gosub sr_sonar					' get sonar reading
     
     if dist_cm >= 15 and dist_cm <= 20 then            ' if an object is within 15 to
          low left_led                                  ' 20 cm then stand still
          low right_led
          goto main
     endif
     
     if dist_cm > 20 then                             ' if an object is less than 10 cm
          high left_led                               ' turn on left LED
          low right_led                               ' turn off right LED
          gosub walk_forward			      ' walk towards object
     endif
     
      if dist_cm < 15 then 
          low left_led                                ' turn off left LED
          high right_led			      ' turn on right LED
          gosub Walk_reverse			      ' walk away from object
     endif    

Goto main

end 

walk_forward:    
		sound portb.4,[90,1,80,2,125,1,90,2,100,2]
		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:
		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
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 
         dist_cm = (dist_raw/conv_cm)		' Convert raw data into centimeters
         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