'{$STAMP BS2}
'BoE_CoP.bs2
'Copyright 2002 Blue Bell Design Inc.
'Simple program for BBDI "Libby" robot modified for BoE-Bot with Co-Processor

' -----[ 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
Timer_Exp	VAR	in11		' Timer Expired I/O pin from BBDI Co-Processor
Rst_Co_Proc	CON 	12		' pin for reset to BBDI Co-Processor when wired
BAUD		CON	84		' 2400 BAUD => BS2 = 396, BS2p = 1021 = CoProc V1.0
					' 9600 BAUD => BS2 = 84, BS2p = 240
					' CoProc V1.1 pin 21 pulled up = 2400 Baud
					' CoProc V1.1 pin 21 grounded = 9600 Baud

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

SerDIn		VAR	BYTE		'gets back Co-Processor info

low_batt_flag 	VAR bit		'flag to remember that the battery voltage is too low			

port 		VAR bit			'Variable to select state of I/O
					'if it is 0 use MAINIO
					'if it is 1 use AUXIO
' -----[ Initialization Code ]--------------------------------------------------

init:					'do initializations

	low_batt_flag = 0		'clear the flag in preparation


	LOW Rst_Co_Proc				'Reset Co-Processor - wired to port.
	PAUSE	20				'Needed when Brownout is not enabled!
	HIGH Rst_Co_Proc	


	SEROUT To_Co_Proc, BAUD, [117]	'initialize for robot mode

'	Write bot_control - this byte controls the Subsumption Architecture.
'	The priorities from lowest to highest are shown below.
'	The lowest level is randomly wandering around.
'	Just above that is level 1, you write the direction and duration for level 1
'	 into the co-Processor registers and it gets integrated into the Subsumption
'	 calculations.
'	Then vision from the IR Proximity Detector.
'	Level 3 is like Level 1
'	bumpers next,
'	Finally the highest priority is level 5. 
'	The bits in the bot_control byte enable each of the levels when they are a 1.
'	Ball Bearing determines the relative directions vs pulse width. BoE-Bot doesn't
'	 have ball bearing servos so it is set to 0. If it is incorrectly set, the robot 
'	 runs backward.
'	do_bot enables the subsumption engine.

	SEROUT To_Co_Proc, BAUD, [218]		'command to write bot_control
	SEROUT To_Co_Proc, BAUD, [$55]			'run the robot!
		' 5 = Ball Bearing = 0; 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:

' First is a battery checker routine - you don't need it to run the robot but it is helpful
'   Don't forget, for 2p40 code, the following I/O is on AUXIO in case you switched it later.

	SEROUT To_Co_Proc, BAUD, [120]		'Read first byte of A/D channel 0 command
	SERIN Frm_Co_Proc, BAUD, [SerDIn]		'Get the MS byte.

	IF SerDIn > $98 THEN clr_batt_flag	'is the regulator voltage 5.97 volts or more?
						' this will need changed for 4 and 5 cell power
						' supplies.

	IF low_batt_flag = 1 THEN check_timers	'already have timer set up
	LOW 15				'Light Mode0 LED on Port
'	write timer 0			set up retriggerable timer for LED flashing
	SEROUT To_Co_Proc, BAUD, [216]		'command to load Timer 0 in retrig mode
	SEROUT To_Co_Proc, BAUD, [25]		'0.5 sec = 25*20ms
	low_batt_flag = 1			'set the flag that will flash the LED
	GOTO check_timers

clr_batt_flag:
	IF low_batt_flag = 0 THEN check_timers	'already shut off LED and cleared timer 0
	HIGH 15				'turn off Mode0 LED on Port
	SEROUT To_Co_Proc, BAUD, [216]		'command to load Timer 0 in retrig mode
	SEROUT To_Co_Proc, BAUD, [0]		'turn off timer 0
	low_batt_flag = 0			'clear the flag to stop flashing Mode0 LED

check_timers:

	IF Timer_Exp = 0 THEN Done_Timers		'Check for timer not complete yet

							'read timeout alarm byte to get and clear it
	SEROUT To_Co_Proc, BAUD, [119]			'Send the byte command
	SERIN Frm_Co_Proc, BAUD, [SerDIn]			'Data comes back into SerDIn

	IF low_batt_flag = 0 THEN Chk_Other_Timers
						'don't bother reading if Timer 0 since won't
						'  flash LED anyway
Chk_for_Timer0:

	IF (NOT(SerDIn & $01)) THEN Chk_Other_Timers	'Jump if not Timer 0.
							' Could have been other timers too.
	TOGGLE 15					'toggle Mode0 LED on Port

  
Chk_Other_Timers:
							'Add your code to check if any other
							'timers are in use too.

Done_Timers:					'Go here when all timer work is done
							'Leaves lots of time to do other things - 

	PAUSE 100					'Just killing time in this case so loop isn't
							' constantly reading A/D

	GOTO main					'loop back


STOP