Index: main.c
===================================================================
--- main.c	(revision 1501)
+++ main.c	(revision 1502)
@@ -1,102 +1,127 @@
 #include <dragonfly_lib.h>
-#include <wireless.h>
-#include <xbee.h>
+#include <wl_basic.h>
 
-// GROUP must be between 0 and 15
-#define GROUP 1
-#define TYPE_RED 'r'
-#define TYPE_GREEN 'g'
+/*
+ * The x component of each BOM detector (indexed from 0 to 15)
+ * was calculated using the following formula:
+ *
+ *		x_comp[i] = round(100 * cos ( 2 * pi / 16 * i) )
+ *
+ * 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 x_comp[16] = {
+	100,
+	92,
+	71,
+	38,
+	0,
+	-38,
+	-71,
+	-92,
+	-100,
+	-92,
+	-71,
+	-38,
+	0,
+	38,
+	71,
+	92
+};
 
-void receive_handler(char type, int source, unsigned char* packet, int length)
-{
 
-	orb_set (0,0,0);
-	
-	switch (type)
-	{
-		case TYPE_RED:   orb2_set (255, 0, 0); break;
-		case TYPE_GREEN: orb2_set (0, 255, 0); break;
-	}
-}
+/*
+ * The y component of each BOM detector (indexed from 0 to 15)
+ * was calculated using the following formula:
+ *
+ *		y_comp[i] = round(100 * sin ( 2 * pi / 16 * i) )
+ *
+ * If the BOM detectors were superimposed onto a 2 dimensional Cartesian space,
+ * this effectively calculates the y 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,
+	38,
+	71,
+	92,
+	100,
+	92,
+	71,
+	38,
+	0,
+	-38,
+	-71,
+	-92,
+	-100,
+	-92,
+	-71,
+	-38
+};
 
+int main (void) {
 
-int main(void) {
-    //dragonfly_init(ALL_ON);
-    dragonfly_init(0);
+	/* 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};
 
-	xbee_init ();
+	/* Arrays for storing the weighted x ("Forwardness") and y ("Rightedness")
+	 * components. Calculated by multiplying the intensity by the x and y
+	 * component respectively (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 ("Forwardness") and y ("Rightedness")
+	 * components for the entire robot. */
+	long net_x_comp = 0;
+	long net_y_comp = 0;
 
-    usb_init ();
-    usb_puts ("Startup\r\n");
+	/* Variables used to normalize the net component values */
+	int total_intensity = 0;
+	int normalized_net_x_comp = 0;
+	int normalized_net_y_comp = 0;
 
-    orb_init_pwm ();
+	int i = 0;
 
+	dragonfly_init(ALL_ON);
+	xbee_init();
+	encoders_init();
 
+	orbs_set_color(BLUE, GREEN);
+	delay_ms(1000);
+	orbs_set_color(GREEN, BLUE);
+	delay_ms(1000);
+	orbs_set_color(RED, RED);
 
-	if (button2_read ())
-	{
-		// Transmitter
-		usb_puts ("Transmitter\r\n");
+	while (1) {
 
-		orb1_set (255, 0, 0);
-		wl_init();
-		orb2_set (255, 0, 0);
+		net_x_comp = 0;
+		net_y_comp = 0;
+		total_intensity = 0;
 
-		while (1)
-		{
-			wl_send_global_packet (GROUP, TYPE_RED, "Test\r", 5, 0);
-			wl_do();
-			delay_ms (100);
-		
-			wl_send_global_packet (GROUP, TYPE_GREEN, "Test\r", 5, 0);
-			wl_do();
-			delay_ms (100);
+		bom_refresh(BOM_ALL);
+		for (i = 0; i < 16; i++) {
+			intensity[i] = 255 - bom_get(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];
 		}
-	}
-	else
-	{
-		// Receiver
-		usb_puts ("Receiver\r\n");
-		
-		orb1_set (0, 255, 0);
-		wl_init();
-		orb2_set (0, 255, 0);
-		
-		usb_puts ("PAN ID: ");  usb_puti (xbee_get_pan_id ());  usb_puts ("\r\n");	// -1
-		usb_puts ("Channel: "); usb_puti (xbee_get_channel ()); usb_puts ("\r\n");	// 0
-		usb_puts ("Address: "); usb_puti (xbee_get_address ()); usb_puts ("\r\n");	// 4/8
 
-		PacketGroupHandler handler = {GROUP, NULL, NULL, &receive_handler, NULL};
-		wl_register_packet_group(&handler);
-		
-		while (1)
-		{
-			delay_ms (10);
-			wl_do();
-		}
-	}
+		normalized_net_x_comp = net_x_comp / total_intensity;
+		normalized_net_y_comp = net_y_comp / total_intensity;
 
-	// +++ => ok
-	// ATVR: Version
-	// ATMY: ID
-	// ATCH: Channel (default C)
-	// ATWR: Write
-	// ATID: PAN ID
-	// ATCN: continue (?)
-	
-	// Channel PAN ID // GROUP MESSAGETYPE
-	
-	//Send a packet to a specific robot in any PAN. 
-	//void wl_send_robot_to_robot_global_packet (char group, char type, char *data, int len, int dest, char frame)
+		delay_ms(500);
 
-	//Send a packet to a specific robot in our PAN. 
-	//void wl_send_robot_to_robot_packet (char group, char type, char *data, int len, int dest, char frame)
+		usb_puts("x: ");	
+		usb_puti(normalized_net_x_comp);
+		usb_puts("\ty: ");
+		usb_puti(normalized_net_y_comp);
+		usb_puts("\n\n");
 
-	//Send a packet to all robots. 
-	//void wl_send_global_packet (char group, char type, char *data, int len, char frame)
+	}
 
-	//Send a packet to all robots in our PAN. 
-	//void wl_send_pan_packet (char group, char type, char *data, int len, char frame)
+	while(1);
 
 }
-
