Index: bom.c
===================================================================
--- bom.c	(nonexistent)
+++ bom.c	(revision 1736)
@@ -0,0 +1,651 @@
+/**
+ * Copyright (c) 2007 Colony Project
+ * 
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ * 
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ **/
+
+
+/**
+ * @file bom.c
+ * @brief Implementation for using the BOM
+ *
+ * Contains functions for using the Bearing and Orientation Module (BOM)
+ *
+ * @author Colony Project, CMU Robotics Club
+ **/
+
+#include "bom.h"
+#include "dio.h"
+#include "serial.h"
+#include "analog.h"
+
+//On the original BOM1.0, the emmitter angular order does not match the analog mux order
+//so you need to iterate through the mux index in the following order if you want to get
+//the detector readings in order:
+static const char lookup[16] = {7,6,5,0xe,1,4,3,2,0xf,0,0xd,8,0xc,0xb,9,0xa};
+
+
+/* *****************************
+ * BOM Vector Component Tables *
+ **************************** **/
+
+/*
+ * The x component of each BOM detector (indexed from 0 to 15)
+ * was calculated using the following formula:
+ *
+ *		x_comp[i] = fix(25 * cos ( 2 * pi / 16 * i) )
+ *
+ * where "fix" rounds towards 0. If the BOM detectors were superimposed
+ * onto a 2 dimensional Cartesian space, this effectively calculates the
+ * x component of the emitter vector where emitter 0 corresponds to an
+ * angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
+ */
+static const signed int x_comp[16] = {
+	25,
+	23,
+	17,
+	9,
+	0,
+	-9,
+	-17,
+	-23,
+	-25,
+	-23,
+	-17,
+	-9,
+	0,
+	9,
+	17,
+	23
+};
+
+
+/*
+ * The y component of each BOM detector (indexed from 0 to 15)
+ * was calculated using the following formula:
+ *
+ *		y_comp[i] = fix(25 * sin ( 2 * pi / 16 * i) )
+ *
+ * where "fix" rounds towards 0. If the BOM detectors were superimposed
+ * onto a 2 dimensional Cartesian space, this effectively calculates the
+ * x component of the emitter vector where emitter 0 corresponds to an
+ * angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
+ */
+static signed int y_comp[16] = {
+	0,
+	9,
+	17,
+	23,
+	25,
+	23,
+	17,
+	9,
+	0,
+	-9,
+	-17,
+	-23,
+	-25,
+	-23,
+	-17,
+	-9
+};
+
+// internal function prototypes
+static void bom_select(char which);
+
+/*
+ Bk R Y (Analog)
+---------
+ Green
+ Blue
+ White
+---------
+ Blue
+ White
+*/
+
+
+/*
+the analog pin definitions from dio.h DO NOT work here,
+so we must use PF0 from avrgcc (as opposed to _PIN_F0).
+BUT the dio pin definitions from dio.h must be used (no PE...).
+
+also, _PIN_E2 is initialized to high for some reason,
+which turns the BOM on when the robot is turned on.
+WORK-AROUND: call digital_output(_PIN_E2,0) at some point.
+
+*/
+
+#define MONKI PF0         //analog (yellow)
+//------------------------//
+#define MONKL _PIN_E2     //green
+#define MONK1 _PIN_E3     //blue
+#define MONK0 _PIN_E4     //white
+//------------------------//
+#define MONK3 _PIN_E6     //blue
+#define MONK2 _PIN_E7     //white
+
+#define BOM_VALUE_THRESHOLD 150 //200
+#define NUM_BOM_LEDS 16
+
+/*
+  *The following pin definitions are for the BOM v1.5
+  */
+
+#define BOM_MODE	_PIN_E2	//dio0
+#define BOM_STROBE	_PIN_E3	//dio1
+
+#define BOM_DATA	_PIN_A0 //servo0
+#define BOM_CLOCK	_PIN_A1	//servo1
+
+#define BOM_S0		_PIN_E5	//dio3
+#define BOM_S1		_PIN_E4	//dio2
+#define BOM_S2		_PIN_E7	//dio4
+#define BOM_S3		_PIN_E6	//dio5
+#define BOM_OUT		PF0		//analog(yellow)
+
+/**
+ * @defgroup bom BOM (Bearing and Orientation Module)
+ * @brief Functions for dealing with the BOM.
+ *
+ * The Bearing and Orientation Module / Barrel of Monkeys / BOM
+ * is a custom sensor designed and built by the Colony Project.
+ * It consists of a ring of 16 IR emitters and 16 IR detectors.
+ * The BOM is most often use to determine the direction of other
+ * robots. This module contains functions for controlling the BOM.
+ *
+ * Include bom.h to access these functions.
+ *
+ * @{
+ **/
+
+static unsigned int bom_val[NUM_BOM_LEDS];
+static volatile char bom_type = BOM10;
+static int select_pins[4];
+static int analog_pin;
+
+/**
+ * Initializes the BOM.
+ * Call bom_init before reading bom values or turning bom leds.
+ *
+ * @bugs INCOMPLETE - No utilization of BOM1.5 RSSI capability. Probably leave this out
+ * until Cornell and Pras return
+ * 
+ * @see bom_refresh, bom_leds_on, bom_leds_off
+ **/
+void bom_init(char type) {
+    bom_type = type;
+    
+    switch(bom_type) {
+    case BOM10:
+		select_pins[0] = MONK0; 
+		select_pins[1] = MONK1;
+		select_pins[2] = MONK2;
+		select_pins[3] = MONK3;
+		analog_pin = MONKI;
+        break;
+    case BOM15:
+        //Sets BOM1.5 to normal [BOM] mode
+        digital_output(BOM_MODE, 0);
+		select_pins[0] = BOM_S0; 
+		select_pins[1] = BOM_S1;
+		select_pins[2] = BOM_S2;
+		select_pins[3] = BOM_S3;
+		bom_set_leds(BOM_ALL);
+		analog_pin = BOM_OUT;
+        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. Must call this before reading BOM values!
+ *
+ *
+ * @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;
+	int loop_was_running = 0;
+    
+	//Check analog loop status
+    if(analog_loop_status() == ADC_LOOP_RUNNING) {
+		loop_was_running = 1;
+		analog_stop_loop();
+	}
+    
+	//Read BOM values
+    for(i = 0; i < NUM_BOM_LEDS; i++) {
+        if(bit_field & 0x1) {
+            bom_select(i);
+            bom_val[i] = analog_get8(analog_pin);
+        }
+        bit_field = bit_field >> 1;
+    }
+    
+	//Restore analog loop status
+	if(loop_was_running)
+		analog_start_loop();
+}
+
+/**
+ * Gets the bom reading from bom_val[which].  Call bom_refresh beforehand to read new bom values.
+ *
+ * @pre must call bom refresh first
+ *
+ * @param which which bom value to return
+ *
+ * @return the bom value
+ *
+ * see bom_refresh
+ **/
+int bom_get(int which) {
+	//return bom_val[which];      deleted because we got rid of the analog loop
+	bom_select(which);
+    return analog_get8(analog_pin);
+}
+
+/** 
+ * Compares all the values in bom_val[] and returns the index to the lowest (max) value element.
+ *
+ * @pre must call bom refresh
+ * @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, i_val;
+    lowest_i = -1;
+    lowest_val = 255;
+    for(i = 0; i < NUM_BOM_LEDS; i++) {
+		i_val = bom_get(i);
+        if(i_val < lowest_val) {
+            lowest_val = i_val;
+            lowest_i = i;
+        }
+    }
+    
+    if(lowest_val < BOM_VALUE_THRESHOLD)
+        return lowest_i;
+    else
+        return -1;
+}
+
+/** 
+ * Compute the net resultant BOM IR vector by scaling each IR unit vector by its intensity
+ *		and summing over all IR LEDs.
+ *
+ * @param v  Pointer to Vector struct to be filled by this function with an x and y net vector
+ *		component.
+ *
+ * @param usrBOMvals  Pointer to array which holds 16 raw BOM readings. Can be used if user
+ *		has already collected BOM information. Otherwise, leave as NULL and a new set of data
+ *		will be collected and used.
+ *
+ * @return  Exit status - Zero for success; negative on error.
+ **/
+int bom_get_vector(Vector* v, int* usrBOMvals) {
+
+	/* Store current BOM readings and use them as a weighting factor */
+	int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+	/* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")
+	 * components. Calculated by multiplying the intensity by the x and y
+	 * component respectively (x and y components are stored in the tables
+	 * above). */
+	int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+	int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+	
+	/* Accumulators to sum up the net x ("Rightness") and y ("Forwardness")
+	 * components for the entire robot. */
+	long net_x_comp = 0;
+	long net_y_comp = 0;
+
+	int i = 0;
+
+	/* BOM intensity is actually measured as more intense = closer to 0 */
+	if (usrBOMvals) {
+		/* Use BOM values collected by user */
+		for (i = 0; i < 16; i++) {
+			intensity[i] = 255 - usrBOMvals[i];
+		}
+	} else {
+		/* Collect new set of BOM data */
+		bom_refresh(BOM_ALL);
+		for (i = 0; i < 16; i++) {
+			intensity[i] = 255 - bom_get(i);
+		}
+	}
+
+	/* Calculate weighted vector components and accumulate vector sum */
+	for (i = 0; i < 16; i++) {
+		weighted_x_comp[i] = intensity[i] * x_comp[i];
+		weighted_y_comp[i] = intensity[i] * y_comp[i];
+		net_x_comp += weighted_x_comp[i];
+		net_y_comp += weighted_y_comp[i];
+	}
+
+	/* Fill the Vector struct */
+	v->x = net_x_comp;
+	v->y = net_y_comp;
+
+	return 0;
+
+}
+
+/** 
+ * Compute the normalized net resultant BOM IR vector by scaling each IR unit vector by its
+ *		intensity and summing over all IR LEDs.
+ *
+ * @param v  Pointer to Vector struct to be filled by this function with an x and y net vector
+ *		component.
+ *
+ * @param usrBOMvals  Pointer to array which holds 16 raw BOM readings. Can be used if user
+ *		has already collected BOM information. Otherwise, leave as NULL and a new set of data
+ *		will be collected and used.
+ *
+ * @return  Exit status - Zero for success; negative on error.
+ **/
+int bom_get_norm_vector(Vector* v, int* usrBOMvals) {
+
+	/* Store current BOM readings and use them as a weighting factor */
+	int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+	/* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")
+	 * components. Calculated by multiplying the intensity by the x and y
+	 * component respectively (x and y components are stored in the tables
+	 * above). */
+	int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+	int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+	
+	/* Accumulators to sum up the net x ("Rightness") and y ("Forwardness")
+	 * components for the entire robot. */
+	long net_x_comp = 0;
+	long net_y_comp = 0;
+
+	/* Variables used to normalize the net component values */
+	int total_intensity = 0;
+	int normalized_net_x_comp = 0;
+	int normalized_net_y_comp = 0;
+
+	int i = 0;
+
+	/* BOM intensity is actually measured as more intense = closer to 0 */
+	if (usrBOMvals) {
+		/* Use BOM values collected by user */
+		for (i = 0; i < 16; i++) {
+			intensity[i] = 255 - usrBOMvals[i];
+		}
+	} else {
+		/* Collect new set of BOM data */
+		bom_refresh(BOM_ALL);
+		for (i = 0; i < 16; i++) {
+			intensity[i] = 255 - bom_get(i);
+		}
+	}
+
+	/* Calculate weighted vector components and accumulate vector sum */
+	for (i = 0; i < 16; i++) {
+		weighted_x_comp[i] = intensity[i] * x_comp[i];
+		weighted_y_comp[i] = intensity[i] * y_comp[i];
+		net_x_comp += weighted_x_comp[i];
+		net_y_comp += weighted_y_comp[i];
+		total_intensity += intensity[i];
+	}
+
+	/* Normalize the resultant vector components by the total intensity */
+	if (total_intensity > 0) {
+		normalized_net_x_comp = net_x_comp / total_intensity;
+		normalized_net_y_comp = net_y_comp / total_intensity;
+	}
+
+	/* Fill the Vector struct */
+	v->x = normalized_net_x_comp;
+	v->y = normalized_net_y_comp;
+
+	return 0;
+
+}
+
+/** 
+ * Print a histogram which shows the current BOM intensity values for each of the 16 BOM IR
+ *		sensors. The function will attempt to send the histogram data over USB.
+ *
+ * @param curBOMvals  Pointer to an array of the current BOM values (the array must have
+ *		length 16). Use this to print values you have already collected. Otherwise pass in NULL
+ *		and bom_refresh() will be called and the current BOM intensity values will be collected.
+ * @return  Exit status - Zero for success; negative on error.
+ **/
+int bom_print_usb(int* usrBOMvals) {
+
+	int i, j, max = -1;
+	int curVals[16];
+	int* prtValPtr;
+
+	if (usrBOMvals) {
+		/* Use BOM values collected by user */
+		prtValPtr = usrBOMvals;
+
+		/* Find max BOM value from users values */
+		for (i = 0; i < 16; i++) {
+			if (max < prtValPtr[i])
+				max = prtValPtr[i];
+		}
+	} else {
+		/* Refresh and make sure the table is updated */
+		bom_refresh(BOM_ALL);
+
+		/* Record values into an array */
+		for (i = 0; i < 16; i++) {
+			curVals[i] = bom_get(i);
+			if (max < curVals[i])
+				max = curVals[i];
+		}
+
+		/* Use the current set of collected values */
+		prtValPtr = curVals;
+	}
+
+	/* Display results */
+	for (i = 0; i < 16; i++) {
+		
+		usb_puti(i);
+		usb_puts(": ");
+		usb_puti(prtValPtr[i]);
+		usb_putc('\t');
+
+		for (j = 0; j < (int)((max - prtValPtr[i]) / 5); j++) {
+			usb_putc('#');
+		}
+		usb_puts("\r\n");
+	}
+	usb_puts("\r\n");
+
+	return 0;
+
+}
+
+/** 
+ * Computes the weighted average of all the bom readings to estimate the position (and distance) of another robot.
+ *
+ * @pre must call bom refresh
+ * @param dist  pointer to int in which to return the estimated distance to the other robot
+ * @return estimated position of the max bom value element as a fixed point value analogous to 10 times the
+ *        index of the max bom value.  -1 if no value is lower than BOM_VALUE_THRESHOLD.
+ **/
+int bom_get_max10(int *dist) {
+    int i, max;
+    long long mean = 0, sum = 0;
+
+    max = bom_get_max();
+    if (max < 0)
+    {
+        if (dist)
+        {
+            *dist = -1;
+        }
+        return -1;
+    }
+    /* Record values into an array */
+    for (i = 0; i < NUM_BOM_LEDS; i++) {
+        int idx = ((i + (NUM_BOM_LEDS/2 - max) + NUM_BOM_LEDS) % NUM_BOM_LEDS) - (NUM_BOM_LEDS/2 - max);
+        int val = 255 - bom_get(i);
+        mean += idx * val;
+        sum += val;
+    }
+    mean = (mean * 10) / sum;
+    mean = (mean + NUM_BOM_LEDS*10) % (NUM_BOM_LEDS*10);
+
+    if (dist)
+    {
+        *dist = 50 - sum/48;
+    }
+
+    return mean;
+}
+
+/**
+ * Iterates through each bit in the bit_field. If the bit is set, the corresponding emitter will
+ *    be enabled to turn on when bom_on() is called.
+ *    bom_init must be called for this to work. Does nothing if a BOM1.0 is installed
+ *
+ * @param bit_field specifies which leds should be turned on when bom_on is called.  Use BOM_ALL to turn on all bom leds.
+ *    Ex. if 0x0005 is passed, leds 0 and 2 will be turned on.
+ **/
+void bom_set_leds(int bit_field) {
+    int i;
+	unsigned int mask = 1<<(NUM_BOM_LEDS-1);
+	switch(bom_type) {
+    case BOM10:
+        //TODO: put an assert here to alert the user that this should not be called
+        break;
+		
+    case BOM15:
+	    for(i=NUM_BOM_LEDS; i>0; i--)
+	    {
+		    //set the current bit, sending MSB first
+		    digital_output(BOM_DATA, bit_field&mask);
+		    //then pulse the clock
+		    digital_output(BOM_CLOCK, 1);
+		    digital_output(BOM_CLOCK, 0);
+			mask = mask>>1;
+	    }
+        break;
+		
+    case RBOM:
+        //add rbom code here
+        break;
+    }
+}
+
+
+/**
+ * (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
+ * the direction of a robot flashing its BOM, of only one
+ * robot is currently doing so. analog_init must be called
+ * before this function can be used.
+ *
+ * @return the direction of the maximum BOM reading
+ *
+ * @see analog_init
+ **/
+int get_max_bom(void) {
+    bom_refresh(BOM_ALL);
+    return bom_get_max();
+}
+
+/**
+ * Flashes the BOM.  If using a BOM1.5, only the emitters that have been enabled using
+ * bom_set_leds will turn on.
+ * 
+ * @see bom_off, bom_set_leds
+ **/
+void bom_on(void)
+{
+  switch(bom_type) {
+  case BOM10:
+	digital_output(MONKL, 1);
+	break;
+  case BOM15:
+	digital_output(BOM_STROBE, 1);
+	break;
+  case RBOM:
+	break;
+  }
+}
+
+/**
+ * Turns off all bom leds.
+ * 
+ * @see bom_on
+ **/
+void bom_off(void)
+{
+  switch(bom_type) {
+  case BOM10:
+	digital_output(MONKL, 0);
+	break;
+  case BOM15:
+	digital_output(BOM_STROBE, 0);
+	break;
+  case RBOM:
+	break;
+  }
+}
+
+/** @} **/ //end group
+
+//select a detector to read
+static void bom_select(char which) {
+	if(bom_type == BOM10)
+	  which = lookup[(int)which];
+	
+    if (which&8)
+      digital_output(select_pins[3], 1);
+    else
+      digital_output(select_pins[3], 0);
+
+    if (which&4)
+      digital_output(select_pins[2], 1);
+    else
+      digital_output(select_pins[2], 0);
+
+    if (which&2)
+      digital_output(select_pins[1], 1);
+    else
+      digital_output(select_pins[1], 0);
+
+    if (which&1)
+      digital_output(select_pins[0], 1);
+    else
+      digital_output(select_pins[0], 0);
+	
+}
