root / trunk / code / behaviors / formation_control / circle_spacing / circle_spacing.c @ 1580
History | View | Annotate | Download (2 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 |
struct vector slave_position;
|
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[2]; |
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++>2) |
62 |
{ |
63 |
switch_sending(); |
64 |
sending_counter = 0;
|
65 |
send_buffer[0] = '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 |
|
82 |
bom_get_vector(&slave_position, NULL);
|
83 |
if(slave_position.y/slave_position.x > 10){ |
84 |
|
85 |
//if(bom_get_max()+1==5){
|
86 |
|
87 |
send_buffer[0] = 's'; |
88 |
wl_basic_send_global_packet(42, send_buffer, 2); |
89 |
} |
90 |
} |
91 |
} |
92 |
|
93 |
|
94 |
|
95 |
else // slave |
96 |
{ |
97 |
if(packet_data[0]=='s') stop=1; |
98 |
if(packet_data[0]=='a') switch_sending(); |
99 |
|
100 |
if(sending)
|
101 |
{ |
102 |
/*
|
103 |
motor_l_set(FORWARD,0);
|
104 |
motor_r_set(FORWARD,0);
|
105 |
*/
|
106 |
} |
107 |
|
108 |
else // recieving |
109 |
{ |
110 |
|
111 |
if(stop)
|
112 |
{ |
113 |
motor_l_set(FORWARD,0);
|
114 |
motor_r_set(FORWARD,0);
|
115 |
orb1_set_color(GREEN); |
116 |
} |
117 |
|
118 |
else
|
119 |
{ |
120 |
|
121 |
|
122 |
|
123 |
|
124 |
//bom_get_vector(&slave_position, NULL);
|
125 |
//if(abs(slave_position.x)>2 && (double)slave_position.y/(double)slave_position.x < 0.1){
|
126 |
|
127 |
if(bom_get_max()+1==8){ |
128 |
orb2_set_color(BLUE); |
129 |
motor_r_set(FORWARD,180);
|
130 |
motor_l_set(FORWARD,180);
|
131 |
|
132 |
} |
133 |
else {
|
134 |
orb2_set_color(GREEN); |
135 |
motor_l_set(FORWARD,0);
|
136 |
motor_r_set(FORWARD,180);
|
137 |
} |
138 |
} |
139 |
|
140 |
|
141 |
} |
142 |
|
143 |
|
144 |
} |
145 |
|
146 |
packet_data = wl_basic_do_default(&data_length); |
147 |
delay_ms(10);
|
148 |
} |
149 |
|
150 |
|
151 |
return 0; |
152 |
|
153 |
|
154 |
|
155 |
|
156 |
} |
157 |
|
158 |
|