'control program Picaxe 28X1
'© D Hall & J Chidley 2008
 



main:

symbol left_motor=1000			'sets left motor speed for straight line max 1023
symbol right_motor=1000			'sets right motor speed for straight line max 1023
symbol left_wall=8			'sets left wall detection 
symbol right_wall=8			'sets right wall detection 
symbol front_wall=6			'sets front wall detection at 12cm 
symbol reset_frontwall=12		'resets wheel counter at 8.5cm if deadend is found 
symbol left_straighten=30		'value of left sensor if no straightening required
symbol right_straighten=30		'value of right sensor if no straightening required
symbol straight_before_right=11	'sets distance of short straight before right turn 
symbol angle_right=80			'sets amount of right turn 
symbol straight_after_right=32	'sets distance of short straight after right turn (lower number to travel further)
symbol straight_before_left=11	'sets distance of short straight before left turn 
symbol angle_left=80 			'sets amount of left turn 
symbol straight_after_left=41		'sets distance of short straight after left turn (lower number to travel further)
symbol straight_turnround=47		'distance travelled into deadend before turn round 
symbol angle_turnround=80		'sets amount of turn round 

	
		 low portc 5			'IR leds off
		 setfreq m8				'internal resinator speed
label_D:	 pwmout 1,255,0			'stop right motor
label_14:	 pwmout 2,255,0			'stop left motor
label_1B:	let b5= 20				'mouse start square requires added wheel counts to straight line count
label_22:	high 6				'middle green led on
label_29:	if pin6=1 then label_29		'wait for start button 
		low 6					'middle green led off
		pause 1000		
label_3B:	 pwmout 1,255,right_motor	
		 pwmout 2,255,left_motor
label_49:	if pin0=1 then label_49		'wait for left wheel counter
		 high portc 5			'IR leds on
		readadc  0,b0			'read left wall sensor
		readadc  1,b1			'read front wall sensor
		readadc  2,b2			'read right wall sensor
		 low portc 5			'IR leds off
		if b0> left_wall then label_91
		let b3= 0				'wall config value for current cell
		low 7					'no left wall left red led off
label_A6:	if b2> right_wall then label_B1
		low 4					'no right wall right red led off
label_C6:	if pin0=1 then label_D3		'wait for left wheel counter
		goto label_C6



label_91:	let b3= 1				'wall config value for current cell
		high 7
		goto label_A6

label_B1:	high 4
		let b3=b3+ 2			'wall config value for current cell
		goto label_C6

label_D3:	let b5=b5+ 1					'add one to straight line counter
		if b1> reset_frontwall then label_30B	'front wall detected end of current move
label_E5:	if b3<b4 then label_F7				'has wall config changed
		let b4=b3
label_105:	if b5> 61 then label_131			'straight line wheel counter end of current move
		if b0< left_wall then label_126		'straighten on left wall if available
		if b0> left_straighten then label_153
label_14C:	 pwmout 2,255,850					'slower speed for left motor to straighten mouse
		 pwmout 1,255,right_motor
		goto label_49

label_F7:	let b5= 43						'use change in wall config to correct straight line wheel counter
		let b4= 0						'resets wall config for previous cell
		goto label_105

label_126:	if b2< right_wall then label_3B		'straighten on right wall if available
		if b2< right_straighten then label_153
		goto label_14C


label_131:	gosub direction_check				'end of cell check for next move
		let b4= 0						'resets wall config for previous cell
		goto label_3B

label_153:	 pwmout 1,255,850					'slower speed for right motor to straighten mouse
		 pwmout 2,255,left_motor
		goto label_49

label_30B:	 high portc 5					'IR leds on
		readadc  1,b1					'read front wall sensor
		 low portc 5					'IR leds off
		if b1> reset_frontwall then label_131 	'double check front sensor for timing gate interference
		goto label_E5


direction_check:	
		if b1> front_wall then label_174
		low 6
label_189:	 pwmout 1,255,0					'stop right motor untill next move is decided
		 pwmout 2,255,0					'stop left motor untill next move is decided
		high 0						'tell picaxe 18x ready for communication
label_19E:	if pin4=1 then label_1A9
		goto label_19E

label_174:	high 6						'middle green led on if wall is detected
		let b3=b3+ 4					'wall config walue for current cell
		goto label_189

label_1A9:	low 0
		 serout 0,N4800,(b3)			'output to picaxe 18x 0-7 wall config
		 serin 4,N4800,b6				'input from picaxe 18x 0-3 next maneuver to make
		 pwmout 1,255,right_motor
		 pwmout 2,255,left_motor
		low 0
		if b6= 1 then label_3BF			'go straight on 
		if b6= 2 then label_1BF			'turn right
		if b6= 3 then label_1C8			'turn left
		gosub turn_round				'turn round
label_1BA:	return

label_1BF:	gosub right_turn
		goto label_1BA

label_1C8:	gosub left_turn
		goto label_1BA

label_3BF:	let b5= 0
		goto label_1BA


right_turn:	
		 let b8=straight_before_right
		gosub left_wheel_counter
		 pwmout 1,255,0
		let b8= angle_right
		gosub left_wheel_counter
		let b5= straight_after_right
		return


left_turn:	
		 let b8=straight_before_left
		gosub left_wheel_counter
		 pwmout 2,255,0
		let b8= angle_left
		let b7= 0
label_22F:	if pin3=1 then label_22F		'use right wheel counter as left turn stops left wheel
		let b7=b7+ 1
		if b7=b8 then label_25E
label_253:	if pin3=1 then label_24C
		goto label_253


label_24C:	let b7=b7+ 1
		if b7=b8 then label_25E
		goto label_22F

label_25E:	let b5= straight_after_left
		return


turn_round:	
		let b8= straight_turnround
		let b7= 0
label_33B:	 pwmout 1,255,right_motor
		 pwmout 2,255,left_motor
		 high portc 5				'IR leds on
		readadc  0,b0				'reads left wall to straight on
		 low portc 5
		if b0< left_wall then label_32B
		if b0< left_straighten then label_31B
		 pwmout 1,255,850				'slower speed for right motor to straighten mouse
		goto label_32B
			
label_31B:	 pwmout 2,255,850				'slower speed for left motor to straighten mouse
		
label_32B:	if pin0=1 then label_32B		'wheel count before for straight before turnround
		let b7=b7+ 1
		if b7=b8 then label_35B
label_36B:	if pin0=1 then label_34B
		goto label_36B

label_34B:	let b7=b7+ 1
		if b7=b8 then label_35B
		goto label_33B
		
		
label_35B:	 pwmout 1,255,0				'stop both motors before turnround
		 pwmout 2,255,0
		pause 200
		high 1					'switch relay on
		 pwmout 1,255,right_motor
		 pwmout 2,255,left_motor
		let b8= angle_turnround
		gosub left_wheel_counter		'count turnround
		 pwmout 1,255,0
		 pwmout 2,255,0
		low 1						'switch relay off
		pause 200
		let b8= 10					'distance to travel out of deadend with out straightening
		let b5= 20					'after deadend requires added wheel counts to straight line count
		 pwmout 1,255,right_motor
		 pwmout 2,255,left_motor
		gosub left_wheel_counter
		return


left_wheel_counter:					'count left wheel counter to the value of b8
		let b7= 0					
label_2B6:	if pin0=1 then label_2B6
		let b7=b7+ 1
		if b7=b8 then label_2F0
label_2D3:	if pin0=1 then label_2DE
		goto label_2D3


label_2DE:	let b7=b7+ 1
		if b7=b8 then label_2F0
		goto label_2B6

label_2F0:	return