'************************************************************************
' BUGBOT
'
' Object avoiding hexapod that uses my home-made IR sensor, and a bump
' switch to avoid objects in its way.
' This one walks on six legs.  When IR detects something it chirps.
' Uses my PIC 12C508 IR detector.
' DLC 12/8/98
'************************************************************************

SYMBOL turned = b0 	        'behaviour memory
SYMBOL detct  = b1	        'IR detector
SYMBOL temp1  = b2              'PWM loop counter
SYMBOL r_mot  = b4              'right outer legs
SYMBOL l_mot  = b5              'left outer legs
SYMBOL m_mot  = b6              'center pivot legs
SYMBOL cornered = b7	        'turn about flag
SYMBOL turning = b8             'turning loop variable

SYMBOL RMOT  = 7                'Right outside legs
SYMBOL LMOT  = 6                'Left outside legs
SYMBOL MMOT  = 5		'middle legs
SYMBOL LEFT  = 4                'Right Detect
SYMBOL RIGHT = 3                'Left Detect
SYMBOL DET   = 2		'Enable IR, this acts weird, leave high
SYMBOL CHIRP = 1	        'Beeper "chirps"
SYMBOL BUMP  = 0		'Bump switch

SYMBOL timer =15	        'loop count for servo control

START: 		
	pause 2000      	          'Just wait 2 seconds
testit: 
	cornered = 1		          'start out normal
	detct = pin4 * 2 + pin3           'look up ahead
        'this math order is very important.  
        'the Stamp I does math from left to right,
        'not in AOS order.

	if pin0 = 0 then backup           'bump switch
	if detct = 0 then dobrnch         'only chirp at obstructions

	sound CHIRP,(120,2,160,2,120,2,160,2)  'beep at obstruction
dobrnch:branch detct,(trot,turnright,turnleft,backup)

backup:
	for b8 = 1 to 3
	    l_mot = 170	                'left forward
	    m_mot = 130	                'and lift
	    gosub moveit
	    l_mot = 100	                'left back
	    m_mot = 170	                'pivot on other side
	    gosub moveit
	    r_mot = 120	                'right forward
	    m_mot = 170	                'and lift
	    gosub moveit
	    r_mot = 190	                'right back
	    m_mot = 130	                'pivot on other side
	    gosub moveit
	next b8
	cornered = 7
	turned = turned + 1		'alternate left and right
	if bit0 = 1 then turnright      'its either even '0' or odd '1'

turnleft:
	for b8 = 1 to cornered
	    l_mot = 170	                'left back
	    m_mot = 130
	    gosub moveit
	    r_mot = 190	                'Right forward
	    m_mot = 170
	    gosub moveit
	    l_mot = 100	                'left forward
	    r_mot = 120	                'right back
	    m_mot = 153
	    gosub moveit
	next b8
	goto testit

turnright:
	for b8 = 1 to cornered
	    l_mot = 100	                'left forward
	    m_mot = 130
	    gosub moveit
	    r_mot = 120	                'Right back
	    m_mot = 170
	    gosub moveit
	    l_mot = 170	                'left back
	    r_mot = 190	                'right forward
	    m_mot = 153	                'neutral
	    gosub moveit
	next b8
	goto testit
trot:
	l_mot = 100	                'left forward
	m_mot = 130	                'and lift
	gosub moveit
	l_mot = 170	                'left back
	m_mot = 170	                'pivot on other side
	gosub moveit
	r_mot = 190	                'right forward
	m_mot = 170	                'and lift
	gosub moveit
	r_mot = 120	                'right back
	m_mot = 130	                'pivot on other side
	gosub moveit
	goto testit

moveit: 'This does the PWM for servos
	for temp1 = 1 to timer          'close to 20ms period 
		pulsout RMOT,r_mot
		pulsout LMOT,l_mot
		pulsout MMOT,m_mot
		pause 15
	next
	return