root / trunk / code / behaviors / formation_control / circle_spacing / circle_spacing.c @ 1564
History | View | Annotate | Download (1.58 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wl_basic.h> |
3 |
#include<encoders.h> |
4 |
|
5 |
|
6 |
int master = 0; |
7 |
int sending = 0; |
8 |
int stop = 0; |
9 |
|
10 |
|
11 |
void switch_sending ()
|
12 |
{ |
13 |
if(sending)
|
14 |
{ |
15 |
sending = 0;
|
16 |
bom_off(); |
17 |
} |
18 |
else
|
19 |
{ |
20 |
sending = 1;
|
21 |
bom_on(); |
22 |
} |
23 |
} |
24 |
|
25 |
|
26 |
|
27 |
int main(void) |
28 |
{ |
29 |
|
30 |
|
31 |
|
32 |
int sending_counter = 0; |
33 |
|
34 |
/* Initialize dragonfly board */
|
35 |
|
36 |
dragonfly_init(ALL_ON); |
37 |
xbee_init(); |
38 |
wl_basic_init_default(); |
39 |
wl_set_channel(12);
|
40 |
int data_length;
|
41 |
unsigned char *packet_data; |
42 |
char send_buffer;
|
43 |
|
44 |
|
45 |
if(master) orb1_set_color(RED);
|
46 |
else orb1_set_color(BLUE);
|
47 |
|
48 |
while(1) |
49 |
{ |
50 |
|
51 |
|
52 |
|
53 |
|
54 |
bom_refresh(BOM_ALL); |
55 |
|
56 |
|
57 |
|
58 |
if(master)
|
59 |
{ |
60 |
|
61 |
if(sending_counter++>100) |
62 |
{ |
63 |
switch_sending(); |
64 |
sending_counter = 0;
|
65 |
send_buffer = 'a';
|
66 |
wl_basic_send_global_packet(42, send_buffer, 1); |
67 |
} |
68 |
|
69 |
|
70 |
if(sending)
|
71 |
{ |
72 |
|
73 |
|
74 |
|
75 |
|
76 |
|
77 |
} |
78 |
|
79 |
else // recieving |
80 |
{ |
81 |
if(bom_get_max()+1==5) |
82 |
{ |
83 |
send_buffer = 's';
|
84 |
wl_basic_send_global_packet(42, send_buffer, 1); |
85 |
} |
86 |
} |
87 |
} |
88 |
|
89 |
|
90 |
|
91 |
else // slave |
92 |
{ |
93 |
if(packet_data[0]=='s') stop=1; |
94 |
if(packet_data[0]=='a') switch_sending(); |
95 |
|
96 |
if(sending)
|
97 |
{ |
98 |
motor_l_set(FORWARD,0);
|
99 |
motor_r_set(FORWARD,0);
|
100 |
} |
101 |
|
102 |
else // recieving |
103 |
{ |
104 |
|
105 |
if(stop)
|
106 |
{ |
107 |
} |
108 |
|
109 |
else
|
110 |
{ |
111 |
|
112 |
|
113 |
if(bom_get_max()+1==8){ |
114 |
orb1_set_color(RED); |
115 |
motor_r_set(FORWARD,180);
|
116 |
motor_l_set(FORWARD,180);
|
117 |
|
118 |
} |
119 |
else {
|
120 |
motor_l_set(FORWARD,0);
|
121 |
motor_r_set(FORWARD,180);
|
122 |
} |
123 |
} |
124 |
|
125 |
|
126 |
} |
127 |
|
128 |
|
129 |
} |
130 |
|
131 |
packet_data = wl_basic_do_default(&data_length); |
132 |
delay_ms(10);
|
133 |
} |
134 |
|
135 |
|
136 |
return 0; |
137 |
|
138 |
|
139 |
|
140 |
|
141 |
} |
142 |
|
143 |
|