root / trunk / code / behaviors / formation_control / circle_spacing / circle_spacing.c @ 1626
History | View | Annotate | Download (3.6 KB)
1 | 1558 | gnagaraj | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wl_basic.h> |
||
3 | #include<encoders.h> |
||
4 | |||
5 | 1564 | gnagaraj | |
6 | int master = 0; |
||
7 | int sending = 0; |
||
8 | int stop = 0; |
||
9 | 1580 | gnagaraj | struct vector slave_position;
|
10 | 1622 | gnagaraj | int desired_max_bom;
|
11 | int bom_max_counter;
|
||
12 | 1564 | gnagaraj | |
13 | 1622 | gnagaraj | |
14 | void set_desired_max_bom(int desired_angle) |
||
15 | { |
||
16 | if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0; |
||
17 | if (desired_angle >= 11 && desired_angle < 33) desired_max_bom = 1; |
||
18 | if (desired_angle >= 33 && desired_angle < 56) desired_max_bom = 2; |
||
19 | if (desired_angle >= 56 && desired_angle < 78) desired_max_bom = 3; |
||
20 | if (desired_angle >= 78 && desired_angle < 101) desired_max_bom = 4; |
||
21 | if (desired_angle >= 101 && desired_angle < 123) desired_max_bom = 5; |
||
22 | if (desired_angle >= 123 && desired_angle < 145) desired_max_bom = 6; |
||
23 | if (desired_angle >= 145 && desired_angle < 167) desired_max_bom = 7; |
||
24 | if (desired_angle >= 167 && desired_angle < 190) desired_max_bom = 8; |
||
25 | if (desired_angle >= 190 && desired_angle < 212) desired_max_bom = 9; |
||
26 | if (desired_angle >= 212 && desired_angle < 235) desired_max_bom = 10; |
||
27 | if (desired_angle >= 235 && desired_angle < 257) desired_max_bom = 11; |
||
28 | if (desired_angle >= 257 && desired_angle < 280) desired_max_bom = 12; |
||
29 | if (desired_angle >= 280 && desired_angle < 302) desired_max_bom = 13; |
||
30 | if (desired_angle >= 302 && desired_angle < 325) desired_max_bom = 14; |
||
31 | if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15; |
||
32 | } |
||
33 | |||
34 | |||
35 | |||
36 | |||
37 | |||
38 | |||
39 | 1564 | gnagaraj | void switch_sending ()
|
40 | { |
||
41 | if(sending)
|
||
42 | { |
||
43 | sending = 0;
|
||
44 | bom_off(); |
||
45 | } |
||
46 | else
|
||
47 | { |
||
48 | sending = 1;
|
||
49 | bom_on(); |
||
50 | } |
||
51 | } |
||
52 | |||
53 | |||
54 | |||
55 | 1558 | gnagaraj | int main(void) |
56 | { |
||
57 | |||
58 | |||
59 | 1564 | gnagaraj | |
60 | int sending_counter = 0; |
||
61 | 1622 | gnagaraj | |
62 | 1558 | gnagaraj | /* Initialize dragonfly board */
|
63 | |||
64 | dragonfly_init(ALL_ON); |
||
65 | xbee_init(); |
||
66 | wl_basic_init_default(); |
||
67 | wl_set_channel(12);
|
||
68 | int data_length;
|
||
69 | 1564 | gnagaraj | unsigned char *packet_data; |
70 | 1571 | gnagaraj | char send_buffer[2]; |
71 | 1564 | gnagaraj | |
72 | |||
73 | 1622 | gnagaraj | if(master) {orb1_set_color(RED); set_desired_max_bom(30);} |
74 | 1564 | gnagaraj | else orb1_set_color(BLUE);
|
75 | |||
76 | 1622 | gnagaraj | |
77 | 1564 | gnagaraj | while(1) |
78 | { |
||
79 | |||
80 | |||
81 | |||
82 | |||
83 | 1558 | gnagaraj | bom_refresh(BOM_ALL); |
84 | |||
85 | |||
86 | 1564 | gnagaraj | |
87 | if(master)
|
||
88 | { |
||
89 | |||
90 | 1626 | gnagaraj | if(sending_counter++>4) // clock for switching the BOMs between the master and slave |
91 | 1564 | gnagaraj | { |
92 | switch_sending(); |
||
93 | sending_counter = 0;
|
||
94 | 1571 | gnagaraj | send_buffer[0] = 'a'; |
95 | 1564 | gnagaraj | wl_basic_send_global_packet(42, send_buffer, 1); |
96 | } |
||
97 | |||
98 | |||
99 | if(sending)
|
||
100 | { |
||
101 | |||
102 | |||
103 | |||
104 | |||
105 | |||
106 | } |
||
107 | |||
108 | else // recieving |
||
109 | { |
||
110 | 1622 | gnagaraj | int max_bom = bom_get_max();
|
111 | usb_puts("bom_get_max : ");
|
||
112 | usb_puti(max_bom); |
||
113 | usb_puts("\n\r");
|
||
114 | 1580 | gnagaraj | |
115 | 1622 | gnagaraj | if(max_bom == desired_max_bom)
|
116 | { |
||
117 | if(bom_max_counter) // only stops the slave if two consecutive boms reading give the desired bom as the max one. Filters the noise. |
||
118 | { |
||
119 | send_buffer[0] = 's'; |
||
120 | wl_basic_send_global_packet(42, send_buffer, 2); |
||
121 | } |
||
122 | bom_max_counter =1;
|
||
123 | } |
||
124 | else bom_max_counter = 0; |
||
125 | 1580 | gnagaraj | |
126 | 1564 | gnagaraj | } |
127 | } |
||
128 | |||
129 | |||
130 | |||
131 | else // slave |
||
132 | { |
||
133 | if(packet_data[0]=='s') stop=1; |
||
134 | if(packet_data[0]=='a') switch_sending(); |
||
135 | |||
136 | if(sending)
|
||
137 | { |
||
138 | 1622 | gnagaraj | |
139 | 1564 | gnagaraj | } |
140 | |||
141 | else // recieving |
||
142 | { |
||
143 | |||
144 | if(stop)
|
||
145 | { |
||
146 | 1571 | gnagaraj | motor_l_set(FORWARD,0);
|
147 | motor_r_set(FORWARD,0);
|
||
148 | orb1_set_color(GREEN); |
||
149 | 1564 | gnagaraj | } |
150 | |||
151 | else
|
||
152 | { |
||
153 | 1622 | gnagaraj | int max_bom = bom_get_max();
|
154 | 1626 | gnagaraj | /*usb_puts("bom_get_max : ");
|
155 | 1622 | gnagaraj | usb_puti(max_bom);
|
156 | 1626 | gnagaraj | usb_puts("/n/r");*/
|
157 | 1580 | gnagaraj | |
158 | 1622 | gnagaraj | |
159 | if(max_bom == 8) |
||
160 | { |
||
161 | 1571 | gnagaraj | orb2_set_color(BLUE); |
162 | 1564 | gnagaraj | motor_r_set(FORWARD,180);
|
163 | motor_l_set(FORWARD,180);
|
||
164 | |||
165 | } |
||
166 | 1622 | gnagaraj | else if(max_bom == 7 || max_bom == 6 || max_bom == 5) |
167 | { |
||
168 | motor_l_set(FORWARD,180);
|
||
169 | motor_r_set(FORWARD,0);
|
||
170 | } |
||
171 | else if(max_bom == -1); |
||
172 | else
|
||
173 | { |
||
174 | orb2_set_color(GREEN); |
||
175 | 1564 | gnagaraj | motor_l_set(FORWARD,0);
|
176 | motor_r_set(FORWARD,180);
|
||
177 | } |
||
178 | } |
||
179 | |||
180 | |||
181 | } |
||
182 | |||
183 | |||
184 | } |
||
185 | |||
186 | packet_data = wl_basic_do_default(&data_length); |
||
187 | delay_ms(10);
|
||
188 | } |
||
189 | 1558 | gnagaraj | |
190 | |||
191 | return 0; |
||
192 | |||
193 | |||
194 | |||
195 | |||
196 | } |
||
197 | |||
198 | |||
199 | 1622 | gnagaraj | |
200 | |||
201 |