Project

General

Profile

Revision 1502

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

View differences:

main.c
1 1
#include <dragonfly_lib.h>
2
#include <wireless.h>
3
#include <xbee.h>
2
#include <wl_basic.h>
4 3

  
5
// GROUP must be between 0 and 15
6
#define GROUP 1
7
#define TYPE_RED 'r'
8
#define TYPE_GREEN 'g'
4
/*
5
 * The x component of each BOM detector (indexed from 0 to 15)
6
 * was calculated using the following formula:
7
 *
8
 *		x_comp[i] = round(100 * cos ( 2 * pi / 16 * i) )
9
 *
10
 * If the BOM detectors were superimposed onto a 2 dimensional Cartesian space,
11
 * this effectively calculates the x component of the emitter vector where
12
 * emitter 0 corresponds to an angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
13
 */
14
static signed int x_comp[16] = {
15
	100,
16
	92,
17
	71,
18
	38,
19
	0,
20
	-38,
21
	-71,
22
	-92,
23
	-100,
24
	-92,
25
	-71,
26
	-38,
27
	0,
28
	38,
29
	71,
30
	92
31
};
9 32

  
10
void receive_handler(char type, int source, unsigned char* packet, int length)
11
{
12 33

  
13
	orb_set (0,0,0);
14
	
15
	switch (type)
16
	{
17
		case TYPE_RED:   orb2_set (255, 0, 0); break;
18
		case TYPE_GREEN: orb2_set (0, 255, 0); break;
19
	}
20
}
34
/*
35
 * The y component of each BOM detector (indexed from 0 to 15)
36
 * was calculated using the following formula:
37
 *
38
 *		y_comp[i] = round(100 * sin ( 2 * pi / 16 * i) )
39
 *
40
 * If the BOM detectors were superimposed onto a 2 dimensional Cartesian space,
41
 * this effectively calculates the y component of the emitter vector where
42
 * emitter 0 corresponds to an angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
43
 */
44
static signed int y_comp[16] = {
45
	0,
46
	38,
47
	71,
48
	92,
49
	100,
50
	92,
51
	71,
52
	38,
53
	0,
54
	-38,
55
	-71,
56
	-92,
57
	-100,
58
	-92,
59
	-71,
60
	-38
61
};
21 62

  
63
int main (void) {
22 64

  
23
int main(void) {
24
    //dragonfly_init(ALL_ON);
25
    dragonfly_init(0);
65
	/* Store current BOM readings and use them as a weighting factor */
66
	int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
26 67

  
27
	xbee_init ();
68
	/* Arrays for storing the weighted x ("Forwardness") and y ("Rightedness")
69
	 * components. Calculated by multiplying the intensity by the x and y
70
	 * component respectively (stored in the tables above). */
71
	int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
72
	int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
73
	
74
	/* Accumulators to sum up the net x ("Forwardness") and y ("Rightedness")
75
	 * components for the entire robot. */
76
	long net_x_comp = 0;
77
	long net_y_comp = 0;
28 78

  
29
    usb_init ();
30
    usb_puts ("Startup\r\n");
79
	/* Variables used to normalize the net component values */
80
	int total_intensity = 0;
81
	int normalized_net_x_comp = 0;
82
	int normalized_net_y_comp = 0;
31 83

  
32
    orb_init_pwm ();
84
	int i = 0;
33 85

  
86
	dragonfly_init(ALL_ON);
87
	xbee_init();
88
	encoders_init();
34 89

  
90
	orbs_set_color(BLUE, GREEN);
91
	delay_ms(1000);
92
	orbs_set_color(GREEN, BLUE);
93
	delay_ms(1000);
94
	orbs_set_color(RED, RED);
35 95

  
36
	if (button2_read ())
37
	{
38
		// Transmitter
39
		usb_puts ("Transmitter\r\n");
96
	while (1) {
40 97

  
41
		orb1_set (255, 0, 0);
42
		wl_init();
43
		orb2_set (255, 0, 0);
98
		net_x_comp = 0;
99
		net_y_comp = 0;
100
		total_intensity = 0;
44 101

  
45
		while (1)
46
		{
47
			wl_send_global_packet (GROUP, TYPE_RED, "Test\r", 5, 0);
48
			wl_do();
49
			delay_ms (100);
50
		
51
			wl_send_global_packet (GROUP, TYPE_GREEN, "Test\r", 5, 0);
52
			wl_do();
53
			delay_ms (100);
102
		bom_refresh(BOM_ALL);
103
		for (i = 0; i < 16; i++) {
104
			intensity[i] = 255 - bom_get(i);
105
			weighted_x_comp[i] = intensity[i] * x_comp[i];
106
			weighted_y_comp[i] = intensity[i] * y_comp[i];
107
			net_x_comp += weighted_x_comp[i];
108
			net_y_comp += weighted_y_comp[i];
109
			total_intensity += intensity[i];
54 110
		}
55
	}
56
	else
57
	{
58
		// Receiver
59
		usb_puts ("Receiver\r\n");
60
		
61
		orb1_set (0, 255, 0);
62
		wl_init();
63
		orb2_set (0, 255, 0);
64
		
65
		usb_puts ("PAN ID: ");  usb_puti (xbee_get_pan_id ());  usb_puts ("\r\n");	// -1
66
		usb_puts ("Channel: "); usb_puti (xbee_get_channel ()); usb_puts ("\r\n");	// 0
67
		usb_puts ("Address: "); usb_puti (xbee_get_address ()); usb_puts ("\r\n");	// 4/8
68 111

  
69
		PacketGroupHandler handler = {GROUP, NULL, NULL, &receive_handler, NULL};
70
		wl_register_packet_group(&handler);
71
		
72
		while (1)
73
		{
74
			delay_ms (10);
75
			wl_do();
76
		}
77
	}
112
		normalized_net_x_comp = net_x_comp / total_intensity;
113
		normalized_net_y_comp = net_y_comp / total_intensity;
78 114

  
79
	// +++ => ok
80
	// ATVR: Version
81
	// ATMY: ID
82
	// ATCH: Channel (default C)
83
	// ATWR: Write
84
	// ATID: PAN ID
85
	// ATCN: continue (?)
86
	
87
	// Channel PAN ID // GROUP MESSAGETYPE
88
	
89
	//Send a packet to a specific robot in any PAN. 
90
	//void wl_send_robot_to_robot_global_packet (char group, char type, char *data, int len, int dest, char frame)
115
		delay_ms(500);
91 116

  
92
	//Send a packet to a specific robot in our PAN. 
93
	//void wl_send_robot_to_robot_packet (char group, char type, char *data, int len, int dest, char frame)
117
		usb_puts("x: ");	
118
		usb_puti(normalized_net_x_comp);
119
		usb_puts("\ty: ");
120
		usb_puti(normalized_net_y_comp);
121
		usb_puts("\n\n");
94 122

  
95
	//Send a packet to all robots. 
96
	//void wl_send_global_packet (char group, char type, char *data, int len, char frame)
123
	}
97 124

  
98
	//Send a packet to all robots in our PAN. 
99
	//void?wl_send_pan_packet (char group, char type, char *data, int len, char frame)
125
	while(1);
100 126

  
101 127
}
102

  

Also available in: Unified diff