/trunk/code/projects/libdragonfly/bom.c - Diff - Colony - Robotics Club

Revision 1536

Merged new BOM vector functions into the trunk and did a "make dist" to recompile the library. Also brought over C file used to test new BOM
function behavior.

     //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] = {
 ,
 ,
 ,
 ,
 ,
     	-9,
     	-17,
     	-23,
     	-25,
     	-23,
     	-17,
     	-9,
 ,
 ,
 ,
     };
     /*
      * 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] = {
 ,
 ,
 ,
 ,
 ,
 ,
 ,
 ,
 ,
     	-9,
     	-17,
     	-23,
     	-25,
     	-23,
     	-17,
     	-9
     };
     // internal function prototypes
     static void bom_select(char which);
-...
+    }
     /**
      * 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 which can be used if user
      *		has already collected BOM information instead of collecting a new data set from the BOM.
+     *
      * @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 which can be used if user
      *		has already collected BOM information instead of collecting a new data set from the BOM.
+     *
      * @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.
      **/
     void bom_print_usb(int* usrBOMvals) {
     int bom_print_usb(int* usrBOMvals) {
     	int i, j, max = -1;
     	int curVals[16];
-...
     		usb_puts("\r\n");
+    	}
     	usb_puts("\r\n");
     	return 0;
+    }

Also available in: Unified diff

Project

General

Profile

Colony

Revision 1536