'{$STAMP BS2p}
'botstatus.BSP
'Copyright 2002 Blue Bell Design Inc.
'Demo program showing how to read robot status byte on BBDI "Libby" robot
'
' -----[ I/O Definitions ]------------------------------------------------------

To_Co_Proc	CON 	10		' pin for data to BBDI Co-Processor
Frm_Co_Proc	CON 	9		' pin for data from BBDI Co-Processor
BAUD		CON	1021		' 2400 BAUD => BS2 = 396, BS2p = 1021
					' 9600 BAUD => BS2 = 84, BS2p = 240
Timer_Exp	VAR	in11		' Timer Expired I/O pin from BBDI Co-Processor
SPKR		CON	12		' Speaker port
LF 		CON 	10		' Line Feed

' -----[ Constants ]------------------------------------------------------------


WRITE_IT	CON	128		'changes an address from read to a write command

' -----[ Variables ]------------------------------------------------------------


SerDIn		VAR	BYTE		'gets back CoP info

port 		VAR 	BIT		'Variable to select state of I/O
					'if it is 0 use MAINIO
					'if it is 1 use AUXIO
port_save 	VAR 	BIT		'a place to save port when changing it temporarily

' -----[ Initialization Code ]--------------------------------------------------

	port = 1
	IOTERM port				'AUXIO - only needed for 2p40

	GOSUB beepit			'beep "Hello!"

	SEROUT To_Co_Proc, BAUD, [116]
	SEROUT To_Co_Proc, BAUD, [116]		'Reset Co-Processor

'	We would normally initialize for robot operation. That would start the IRPD
'	 (IR Proximity Detect). Here we don't want the servos to start.
'	We just want to turn on the IRPD so we can look at what is happening.
'	Notice that the IRPD LEDs can still light IF you set the bot_control.

	SEROUT To_Co_Proc, BAUD, [(WRITE_IT+86),27]	'write IR Freq Register
							'default = 23; max sensitivity = 27; min = 20
							'Writing a nonzero value enables the IRPD
							'period = 15.6 + 0.4*counts (us) (23 = 24.8uS)
							'Change the 23 to other values and see the effect
							' on sensitivity.

	SEROUT To_Co_Proc, BAUD, [(218)]		'command to write bot_control
	SEROUT To_Co_Proc, BAUD, [$40]			'run the IRPD display LEDs but not the robot.
							'without this command the display LEDs 
							'   won't light.
		' D = Ball Bearing = 1; Show Vision = 1; level 5 = 0; do_bumpers = 1;
		' 5 = level 3 = 0; do_vision = 1; level 1 = 0; do_bot = 1




' -----[ Main Code ]------------------------------------------------------------

main:

'	read_CoProc				'read the value from the CoProcessor

	SEROUT To_Co_Proc, BAUD, [118]		'Send the byte command
	SERIN Frm_Co_Proc, BAUD, [SerDIn]	'Data comes back into SerDIn

	DEBUG"Bot Status = "

	IF SerDIn & $20 THEN show_see_L
	DEBUG"         "			'don't see anything on left.
	GOTO around1

show_see_L:
	DEBUG"See Left "			'see something on left.

around1:
	IF SerDIn & $40 THEN show_see_R		'Send values to PC
	DEBUG"          "			'don't see anything on right.
	GOTO around2

show_see_R:					'see something on right.
	DEBUG"See Right "

around2:
	IF SerDIn & $04 THEN show_bmp_L
	DEBUG"          "		'bumper not hit on left.
	GOTO around3

show_bmp_L:
	DEBUG"Bump Left "		'bumper was hit on left.

around3:
	IF SerDIn & $08 THEN show_bmp_R
	DEBUG"           "		'bumper not hit on right.
	GOTO around4

show_bmp_R:
	DEBUG"Bump Right "		'bumper was hit on right.

around4:
	DEBUG"  -> rcvd = " , HEX2 SerDIn,CR,LF

	PAUSE 1000			'give your eyes time to look at the screen

	GOTO main


STOP



'========================================================================
'	SUBROUTINES BELOW
'========================================================================

beepit:				'Beep the speaker

	port_save = port
	port = 1
	IOTERM port				'switch to AUXIO
	freqout SPKR,500,2000,4000	'Beep the speaker
	DEBUG"Speaker should have beeped",CR,LF
	port = port_save			'restore value
	IOTERM port				'use it

	RETURN