'**********************************************************************
' maze-solving.bas
' Walking robot maze solving using wall following technique.            
' 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
conv_inch   	con 	15              ' conversion factor for inches
turn            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                  ' look to the right
     gosub turn_right
     next turn

     gosub sr_sonar                     ' get sonar distance measurement

     if dist_inch < 7 then              ' is there a wall to the right?
         for turn = 1 to 5              ' Yes, then turn the robot back    
         gosub turn_left                ' No, then keep the robot facing right
         next turn                      
     endif

     gosub sr_sonar                     ' get sonar distance measurement

     if dist_inch < 7 then              ' is there a wall in front of the robot?
        for turn = 1 to 5               ' Yes, then turn the robot left
        gosub turn_left                 ' No, keep the robot in its current orientation
        next turn
     endif

     for turn = 1 to 5                  ' walk the robot forward 5 cycles
     gosub walk_forward
     next turn

goto main                               

end 

walk_forward:                            ' forward walking 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
		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

turn_left:                                  ' rotate the robot left 18 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 18 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