' BoE_CoP_B.bs2
' Copyright 2003 Blue Bell Design Inc.
' Simple program for BoE-Bot with Co-Processor "Belly" board 7/3/03
' {$STAMP BS2}
' {$PBASIC 2.5}

' NOTE:
'    Plug Left Wheel Servo into Servo0 and
'    Right Wheel Servo into Servo1 ON Co-Processor board
'    Servo ground (Black) is pin closest to PCB. Control (white)is furthest from PCB.

' -----[ I/O Definitions ]------------------------------------------------------


To_Co_Proc      PIN     11              ' pin for data to BBDI Co-Processor
Frm_Co_Proc     PIN     10              ' pin for data from BBDI Co-Processor
Timer_Exp       PIN     9               ' Timer Expired I/O pin from BBDI Co-Processor
Rst_Co_Proc     PIN     8               ' pin for reset to BBDI Co-Processor

FlasherLED      PIN     15              ' Flashing LED to give Stamp something to do!

BAUD            CON     84              ' 9600 BAUD => BS2 = 84, BS2p = 240
                                        ' CoProc V1.1 pin 21 grounded = 9600 Baud

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

SerDIn          VAR     Byte            ' gets back Co-Processor info

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

init:

  LOW Rst_Co_Proc                       ' Reset Co-Processor
  PAUSE  20
  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

                                        ' Setup LED for Stamp to flash
  LOW FlasherLED                        ' turn off LED on Port
  SEROUT To_Co_Proc, BAUD, [216]        ' command to load Timer 0 in retrig mode
  SEROUT To_Co_Proc, BAUD, [25]         ' value to be loaded to Timer 0 = 500 ms

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

main:

check_timers:                           ' only used for Stamp to flash LED

  IF Timer_Exp = 1 THEN                 ' check for timer complete -
                                        ' from here to done_timers only
                                        ' takes about 4 ms and happens
                                        ' once per 500 ms - only 1% load on BS2

    ' 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 (SerDIn & $01) THEN TOGGLE FlasherLED  'toggle LED on Port

    ' add your code here if any other timers are in use too.
  ENDIF                                 ' timer work is done
done_timers:

    ' leaves 99% of the time, 96% of the variables and 97% of code space
    ' to do other things - For instance you can
    '   1. Read A/Ds for Sharp IR distance sensors. For wall following?
    '   2. Read Sonar sensors without worrying about servo refreshing.
    '   3. How about a CMU Cam?

  GOTO main          'loop back

STOP