'Robot Program For PICAXE18 microcontroller and L293D
'Tamiya Dual Gearbox at 114.7:1 ratio (Type C) and 56mm wheels

'Opposite motor is off during turns
'If another ratio is used, the motor controls swop.

'No sound

'MCB Mar 2010

'output pin allocation (%76543210)
'76 = Right Motor
'54 = Left Motor
'3  = not used 		'pwmout pin
'2  = LED B			'future
'1  = LED A			'future
'0  = piezo sounder	'future


'input pin allocation
'7  = Right Switch looking from the rear
'6  = Left Switch looking from the rear

'counter allocation
symbol RightCount = b0
symbol LeftCount = b1
symbol ForwardCount = b2 
symbol CornerCount =b3
symbol LastTurn = b4
symbol RightBump = pin7
symbol LeftBump = pin6


Waiting:

'In order to put the robot down, the motors should be off
'Press a bumper switch to start
	
	if RightBump = 1 or LeftBump = 1 then check

goto waiting

check:
	pause 100

main:
	let pins = %10100000		'both motors forward
	ForwardCount = ForwardCount + 1
	
	if RightBump =1 or LeftBump =1 then
		let pins =%00000000	'Stop
		pause 10		'10ms time to settle and check if both touch
	endif
	
	if RightBump = 1 and LeftBump =1 then backturn
	if RightBump = 1 and CornerCount >3 or LeftBump =1 and CornerCount >3 then Corner
	if LeftBump = 1 then left
	if RightBump = 1 then right

	if ForwardCount = 100 then ResetCount
goto main

ResetCount:
	ForwardCount = 0
	RightCount = 0
	LeftCount =0
	CornerCount =0
	LastTurn=0

Goto Main

Left:

	If LastTurn =1 then 
		CornerCount = 0
		else
		CornerCount = CornerCount +1		
	endif
	
	LeftCount = LeftCount +1
	let pins =%01010000	'Reverse
	pause 50
	let pins =%00100000	'Left forward, Right off
	pause 75
	LastTurn =1
	ForwardCount =0

goto main

right:

	If LastTurn =2 then 
		CornerCount = 0
		else
		CornerCount = CornerCount +1		
	endif
	
	RightCount = RightCount +1
	'let pins =%00000000	'Stop
	'pause 25
	let pins =%01010000	'Reverse
	pause 50
	let pins =%10000000	'Right forward, Left off
	pause 75
	LastTurn =2
	ForwardCount =0
	
goto main

backturn:

	If RightCount > LeftCount then	'turn right
		let pins =%01010000		'Reverse
		pause 100
		let pins =%10000000		'Right forward, Left off
		pause 100
	
	else						'turn left
		let pins =%01010000		'Reverse
		pause 100
		let pins =%00100000		'Left forward, Right off
		pause 100
	endif

	LastTurn =0
	ForwardCount =0
	
goto main

Corner:
'we think we've stuck in a corner therefore we need to add something to exit the sequence

If RightCount > LeftCount then	'turn right
	
		let pins =%01010000	'Reverse
		pause 75
		let pins =%10000000	'Right forward, Left off
		pause 200
	
	else					'turn left
		let pins =%01010000	'Reverse
		pause 75
		let pins =%00100000	'Left forward, Right off
		pause 200
	endif
	
	Cornercount=0
goto main