Revision 1564
changes of wednesday 18 nov
trunk/code/behaviors/formation_control/circle_spacing/circle_spacing.c | ||
---|---|---|
2 | 2 |
#include <wl_basic.h> |
3 | 3 |
#include<encoders.h> |
4 | 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 |
|
|
5 | 27 |
int main(void) |
6 | 28 |
{ |
7 | 29 |
|
8 | 30 |
|
31 |
|
|
32 |
int sending_counter = 0; |
|
9 | 33 |
|
10 | 34 |
/* Initialize dragonfly board */ |
11 | 35 |
|
... | ... | |
14 | 38 |
wl_basic_init_default(); |
15 | 39 |
wl_set_channel(12); |
16 | 40 |
int data_length; |
17 |
unsigned char *packet_data=wl_basic_do_default(&data_length); |
|
18 |
int distance=24;//packet_data[0]; |
|
19 |
encoder_rst_dx(RIGHT); |
|
20 |
while(encoder_get_x(RIGHT)<=distance){ |
|
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 |
|
|
21 | 54 |
bom_refresh(BOM_ALL); |
22 |
|
|
23 |
orb1_set_color(BLUE); |
|
24 | 55 |
|
25 |
if(bom_get_max()+1==8){ |
|
26 |
orb1_set_color(RED); |
|
27 |
motor_r_set(FORWARD,180); |
|
28 |
motor_l_set(FORWARD,180); |
|
29 | 56 |
|
30 |
} |
|
31 |
else { |
|
32 |
motor_l_set(FORWARD,0); |
|
33 |
motor_r_set(FORWARD,180); |
|
34 |
} |
|
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 |
} |
|
35 | 134 |
|
36 |
} |
|
37 |
motor_r_set(FORWARD,0); |
|
38 |
motor_l_set(FORWARD,0); |
|
39 |
while(1); |
|
40 | 135 |
|
41 | 136 |
return 0; |
42 | 137 |
|
Also available in: Unified diff