Index: bom.c
===================================================================
--- bom.c	(revision 447)
+++ bom.c	(revision 448)
@@ -38,12 +38,30 @@
 #include "dio.h"
 #include "analog.h"
 
+//TODO: DELETE THIS
+void printi(int i)
+{
+    char c3 = (i % 10) + '0';
+    i /= 10;
+    char c2 = (i % 10) + '0';
+    i /= 10;
+    char c1 = (i % 10) + '0';
+    if (c1 == '0')
+    {
+        c1 = ' ';
+        if (c2 == '0')
+            c2 = ' ';
+    }
+    usb_putc(c1);
+    usb_putc(c2);
+    usb_putc(c3);
+}
+
 //constants
-const int lookup[16] = {7,6,5,0xe,1,4,3,2,0xf,0,0xd,8,0xc,0xb,9,0xa};
+static const char lookup[16] = {7,6,5,0xe,1,4,3,2,0xf,0,0xd,8,0xc,0xb,9,0xa};
 
 // internal function prototypes
-void output_high(int which);
-void output_low(int which);
+static void bom_select(char which);
 
 /*
  Bk R Y (Analog)
@@ -77,7 +95,8 @@
 #define MONK3 _PIN_E6     //blue
 #define MONK2 _PIN_E7     //white
 
-#define BOM_VALUE_THRESHOLD 200
+#define BOM_VALUE_THRESHOLD 200
+#define NUM_BOM_LEDS 16
 
 
 /**
@@ -93,10 +112,148 @@
  * Include bom.h to access these functions.
  *
  * @{
- **/
+ **/
+
+static unsigned int bom_val[NUM_BOM_LEDS];
+static char bom_type = BOM;
+  
+/**
+ * Initializes the BOM.
+ * Call bom_init before reading bom values or turning bom leds.
+ *
+ * @bugs INCOMPLETE - need to fill in init routine for BOM15
+ * 
+ * @see bom_refresh, bom_leds_on, bom_leds_off
+ **/
+void bom_init(char type) {
+    bom_type = type;
+    
+    switch(bom_type) {
+    case BOM:
+        break;
+    case BOM15:
+        break;
+    case RBOM:
+        break;
+    //default:
+    }
+}
+
+/**
+ * Iterates through each bit in the bit_field. For each set bit, sets the corresponding bom select bits
+ *    and updates the corresponding bom value with an analog_get8 reading.  analog_init and bom_init
+ *    must be called for this to work.
+ *
+ *
+ * @param bit_field specifies which elements in bom_val[] should be updated. Use BOM_ALL to refresh all values.
+ *    Ex. if 0x0003 is passed, bom_val[0] and bom_val[1] will be updated.
+ *
+ * @see bom_get
+ **/
+void bom_refresh(int bit_field) {
+    int i;
+    
+    analog_stop_loop();
+    
+    for(i = 0; i < NUM_BOM_LEDS; i++) {
+        if(bit_field & 0x1) {
+            bom_select(lookup[i]);
+            bom_val[i] = analog_get8(MONKI);
+        }
+        bit_field = bit_field >> 1;
+    }
+    
+    analog_start_loop();
+}
+
+/**
+ * Gets the bom reading from bom_val[which].  Call bom_refresh beforehand to read new bom values.
+ *
+ * @param which which bom value to return
+ *
+ * @return the bom value
+ *
+ * see bom_refresh
+ **/
+int bom_get(int which) {
+    return bom_val[which];
+}
+
+/** 
+ * Compares all the values in bom_val[] and returns the index to the lowest (max) value element.
+ *
+ * @return index to the lowest (max) bom value element.  -1 if no value is lower than
+ *    BOM_VALUE_THRESHOLD
+ **/
+int bom_get_max(void) {
+    int i, lowest_val, lowest_i;
+    lowest_i = -1;
+    lowest_val = 255;
+    for(i = 0; i < NUM_BOM_LEDS; i++) {
+        printi(bom_val[i]);
+        usb_puts(" ");
+        if(bom_val[i] < lowest_val) {
+            lowest_val = bom_val[i];
+            lowest_i = i;
+        }
+    }
+    usb_puts("<< BOM_GET_MAX\n\r");
+    
+    if(lowest_val < BOM_VALUE_THRESHOLD)
+        return lowest_i;
+    else
+        return -1;
+}
+
+/**
+ * Iterates through each bit in the bit_field. For each set bit, turns on the corresponding bom led.
+ *    bom_init must be called for this to work. Only works with BOM_ALL if using the original bom.
+ *
+ * @param bit_field specifies which leds should be turned on.  Use BOM_ALL to turn on all bom leds.
+ *    Ex. if 0x0005 is passed, leds 0 and 2 will be turned on.
+ **/
+void bom_leds_on(int bit_field) {
+    switch(bom_type) {
+    case BOM:
+        if(bit_field == BOM_ALL) {
+            digital_output(MONKL, 1);
+        }
+        break;
+    case BOM15:
+        //add bom 1.5 code here
+        break;
+    case RBOM:
+        //add rbom code here
+        break;
+    }
+}
+
+/**
+ * Iterates through each bit in the bit_field. For each set bit, turns off the corresponding bom led.
+ *    bom_init must be called for this to work. Only works with BOM_ALL if using the original bom.
+ *
+ * @param bit_field specifies which leds should be turned off.  Use BOM_ALL to turn off all bom leds.
+ *    Ex. if 0x000B is passed, leds 0 and 3 will be turned off.
+ **/
+void bom_leds_off(int bit_field) {
+    switch(bom_type) {
+    case BOM:
+        if(bit_field == BOM_ALL) {
+            digital_output(MONKL, 0);
+        }
+        break;
+    case BOM15:
+        //add bom 1.5 code here
+        break;
+    case RBOM:
+        //add rbom code here
+        break;
+    }
+}
 
+
 /**
- * Returns the direction of the maximum BOM reading,
+ * (DEPRECATED) Returns the direction of the maximum BOM reading,
  * as an integer in the range 0-15. 0 indicates to the
  * robot's right, while the rest of the sensors are
  * numbered counterclockwise. This is useful for determining
@@ -109,91 +266,50 @@
  * @see analog_init
  **/
 int get_max_bom(void) {
-	int max_bom_temp = 0;
-	int a, i, j, h;
-    h = 255;
-
-	//Turn off the loop so that we can actually use analog8 correctly
-	analog_stop_loop();
-
-	//Iterate through through each LED
-    for (j = 0; j < 16; j++)
-    {
-      i = lookup[j];
-
-      if (i&8)
-        output_high(MONK3);
-      else
-        output_low(MONK3);
-
-      if (i&4)
-        output_high(MONK2);
-      else
-        output_low(MONK2);
-
-      if (i&2)
-        output_high(MONK1);
-      else
-        output_low(MONK1);
-
-      if (i&1)
-        output_high(MONK0);
-      else
-        output_low(MONK0);
-
-      a = analog_get8(MONKI);
-              
-      if (a < h)
-      {
-        h = a;
-        max_bom_temp = j;
-      }
-
-    }
-	
-	//Restart loop now that we are done using analog8
-	analog_start_loop();
-
-	//threshold on the bom analog value.
-	//defined in bom.h
-	// if the analog value read is above the threshold, we cannot see a robot
-	// (remember, low means higher intensity).
-	if(h < BOM_VALUE_THRESHOLD) {
-		return max_bom_temp;
-	}
-	else
-		return -1;
+    bom_refresh(BOM_ALL);
+    return bom_get_max();
 }
 
 /**
- * Flashes the BOM. analog_init must be called before this
- * function can be used.
+ * (DEPRECATED) Turns on all bom leds.
  * 
- * @see bom_off, analog_init
+ * @see bom_off
  **/
 void bom_on(void)
 {
-  output_high(MONKL);
+  bom_leds_on(BOM_ALL);
 }
 
 /**
- * Stops flashing the BOM. analog_init must be called
- * before this function can be used.
+ * (DEPRECATED) Turns off all bom leds.
  * 
- * @see bom_on, analog_init
+ * @see bom_on
  **/
 void bom_off(void)
 {
-  output_low(MONKL);
+    bom_leds_off(BOM_ALL);
 }
 
 /** @} **/ //end group
+
+static void bom_select(char which) {
+      if (which&8)
+        digital_output(MONK3, 1);
+      else
+        digital_output(MONK3, 0);
 
-void output_high(int which) {
-	digital_output(which, 1);
-}
+      if (which&4)
+        digital_output(MONK2, 1);
+      else
+        digital_output(MONK2, 0);
 
-void output_low(int which) {
-	digital_output(which, 0);
-}
+      if (which&2)
+        digital_output(MONK1, 1);
+      else
+        digital_output(MONK1, 0);
 
+      if (which&1)
+        digital_output(MONK0, 1);
+      else
+        digital_output(MONK0, 0);
+}
