/ - Diff - Colony - Robotics Club

Revision 1502

Wrote Push-Pull behavior to print the net BOM intensity vector over serial.

     #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] = {
 ,
 ,
 ,
 ,
 ,
     	-38,
     	-71,
     	-92,
     	-100,
     	-92,
     	-71,
     	-38,
 ,
 ,
 ,
     };
     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] = {
 ,
 ,
 ,
 ,
 ,
 ,
 ,
 ,
 ,
     	-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);
+    }

     # Relative path to the root directory (containing lib directory)
     ifndef COLONYROOT
     COLONYROOT = ../..
     COLONYROOT = ../../..
     endif
     # Target file name (without extension).

Also available in: Unified diff

Project

General

Profile

Colony

Revision 1502