root / trunk / code / projects / object_manipulation / obj_detect_swarm / driver.c @ 775
History | View | Annotate | Download (4.97 KB)
1 |
/** driver for bfs swarming
|
---|---|
2 |
execute bfs behavior
|
3 |
*/
|
4 |
|
5 |
#define COLOR_DETECT // comment out if color detect hardware is unavailable |
6 |
|
7 |
#include <dragonfly_lib.h> |
8 |
#include <wireless.h> |
9 |
#include <wl_token_ring.h> |
10 |
#include <dio.h> |
11 |
#include <analog.h> |
12 |
#include "bfs_fsm.h" |
13 |
#include "smart_run_around_fsm.h" |
14 |
|
15 |
|
16 |
//A6
|
17 |
#define RED_LED PIN_ADC4
|
18 |
//A8
|
19 |
#define GREEN_LED PIN_ADC5
|
20 |
//A7
|
21 |
#define BLUE_LED PIN_ADC6
|
22 |
|
23 |
|
24 |
// COLOR_NOTHING {255,255,255}
|
25 |
#define COLOR_WALL 2 // ratio of blue/green |
26 |
#define COLOR_GREEN 1 |
27 |
|
28 |
|
29 |
#define SWARM_GROUP 15 |
30 |
#define SWARM_ACTION 16 |
31 |
|
32 |
|
33 |
/* swarm wireless packet handlers */
|
34 |
void swarm_receive (char type, int source, unsigned char* packet, |
35 |
int length);
|
36 |
PacketGroupHandler swarmHandler = {SWARM_GROUP, NULL, NULL, |
37 |
&swarm_receive, NULL};
|
38 |
|
39 |
|
40 |
int main(void) { |
41 |
// enable everything
|
42 |
dragonfly_init(ALL_ON); |
43 |
orb_enable(); |
44 |
orb_init(); |
45 |
orb_set_color(PURPLE); |
46 |
wl_init(); |
47 |
wl_set_channel(0xF); // set wireless channel |
48 |
|
49 |
// register swarm packet
|
50 |
wl_register_packet_group(&swarmHandler); |
51 |
|
52 |
wl_token_ring_register(); |
53 |
wl_token_ring_join(); // join token ring
|
54 |
usb_init(); |
55 |
|
56 |
usb_puts("start\n\r");
|
57 |
|
58 |
|
59 |
#ifdef COLOR_DETECT
|
60 |
/* set up color detect */
|
61 |
int rgb[3] = {0,0,0}; |
62 |
int rgb_old1[3] = {-1,0,0}; |
63 |
int rgb_old2[3] = {-1,0,0}; |
64 |
int rgb_avg[3] = {0,0,0}; |
65 |
|
66 |
//long int distance_red;
|
67 |
//long int distance_green;
|
68 |
//long int distance_blue;
|
69 |
|
70 |
int ratio_gb;
|
71 |
|
72 |
//int blue[3] = {255,245,249};
|
73 |
//int red[3] = {190,122,126};
|
74 |
//int green[3] = {118, 80, 86};
|
75 |
|
76 |
analog_init(ADC_START); |
77 |
|
78 |
#else
|
79 |
|
80 |
run_around_init(); |
81 |
|
82 |
#endif
|
83 |
|
84 |
int object = 0; |
85 |
|
86 |
|
87 |
|
88 |
while(1) { |
89 |
wl_do(); // do wireless
|
90 |
|
91 |
#ifdef COLOR_DETECT
|
92 |
// need color sensor code here
|
93 |
digital_output(BLUE_LED, 1);
|
94 |
|
95 |
delay_ms(5);
|
96 |
analog_stop_loop(); |
97 |
rgb[2] = analog8(AN9);
|
98 |
analog_start_loop(); |
99 |
|
100 |
digital_output(BLUE_LED,0);
|
101 |
digital_output(RED_LED, 1);
|
102 |
delay_ms(5);
|
103 |
|
104 |
analog_stop_loop(); |
105 |
rgb[0] = analog8(AN9);
|
106 |
analog_start_loop(); |
107 |
|
108 |
digital_output(RED_LED, 0);
|
109 |
digital_output(GREEN_LED,1);
|
110 |
delay_ms(5);
|
111 |
|
112 |
analog_stop_loop(); |
113 |
rgb[1] = analog8(AN9);
|
114 |
analog_start_loop(); |
115 |
|
116 |
digital_output(GREEN_LED,0);
|
117 |
|
118 |
// check with older values
|
119 |
if (rgb_old1[0] != -1 && rgb_old2[0] != -1) { |
120 |
rgb_avg[0] = (rgb[0]+rgb_old1[0]+rgb_old2[0])/3; |
121 |
rgb_avg[1] = (rgb[1]+rgb_old1[1]+rgb_old2[1])/3; |
122 |
rgb_avg[2] = (rgb[2]+rgb_old1[2]+rgb_old2[2])/3; |
123 |
} else {
|
124 |
rgb_avg[0] = rgb[0]; |
125 |
rgb_avg[1] = rgb[1]; |
126 |
rgb_avg[2] = rgb[2]; |
127 |
} |
128 |
|
129 |
if (rgb_old1[0] == -1) { |
130 |
rgb_old1[0] = rgb[0]; |
131 |
rgb_old1[1] = rgb[1]; |
132 |
rgb_old1[2] = rgb[2]; |
133 |
} else {
|
134 |
if (rgb_old1[0] < rgb[0]+20 |
135 |
&& rgb_old1[0] > rgb[0]-20) { |
136 |
rgb_old1[0] = rgb[0]; |
137 |
rgb_old2[0] = rgb_old1[0]; |
138 |
} |
139 |
if (rgb_old1[1] < rgb[1]+20 |
140 |
&& rgb_old1[1] > rgb[1]-20) { |
141 |
rgb_old1[1] = rgb[1]; |
142 |
rgb_old2[1] = rgb_old1[1]; |
143 |
} |
144 |
if (rgb_old1[2] < rgb[2]+20 |
145 |
&& rgb_old1[2] > rgb[2]-20) { |
146 |
rgb_old1[2] = rgb[2]; |
147 |
rgb_old2[2] = rgb_old1[2]; |
148 |
} |
149 |
} |
150 |
|
151 |
// check ratio
|
152 |
ratio_gb = rgb_avg[1] - rgb_avg[2]; |
153 |
if (ratio_gb < -20) |
154 |
ratio_gb = 2;
|
155 |
else if (ratio_gb > 20) |
156 |
ratio_gb = 0;
|
157 |
else
|
158 |
ratio_gb = 1;
|
159 |
|
160 |
if (rgb_avg[0] > 200 && rgb_avg[1] > 200 && rgb_avg[2] > 200) { |
161 |
// we think this is open area
|
162 |
move(170,0); |
163 |
} else if (ratio_gb == COLOR_WALL) { |
164 |
// we think this is a wall, so turn away
|
165 |
move(170,90); |
166 |
delay_ms(400);
|
167 |
move(170,0); |
168 |
} else if (ratio_gb == COLOR_GREEN) { |
169 |
// we think this is a green object, so swarm
|
170 |
object = 1;
|
171 |
move(0,0); |
172 |
} |
173 |
else
|
174 |
move(170,0); // just go forth |
175 |
|
176 |
/*distance_red = 1;
|
177 |
distance_green = rgb[1] - rgb[0];
|
178 |
distance_blue = rgb[2] - rgb[0];
|
179 |
|
180 |
distance_blue = distance_blue * rgb[0]/255;
|
181 |
distance_green = distance_green * rgb[0]/255;*/
|
182 |
|
183 |
usb_puts("R:");
|
184 |
usb_puti(rgb_avg[0]);
|
185 |
usb_puts("\tG:");
|
186 |
usb_puti(rgb_avg[1]);
|
187 |
usb_puts("\tB:");
|
188 |
usb_puti(rgb_avg[2]);
|
189 |
usb_puts("\t\tratio_gb:");
|
190 |
usb_puti(ratio_gb); |
191 |
usb_puts("\r\n");
|
192 |
|
193 |
#else
|
194 |
// do smart run around until find object
|
195 |
run_around_FSM(); |
196 |
//move(170,0);
|
197 |
#endif
|
198 |
|
199 |
/*usb_puts("R:");
|
200 |
usb_puti(distance_red);
|
201 |
usb_puts("\tG:");
|
202 |
usb_puti(distance_green);
|
203 |
usb_puts("\tB:");
|
204 |
usb_puti(distance_blue);
|
205 |
usb_puts("\r\n");*/
|
206 |
|
207 |
if (object || button2_read()) { // if we find the object |
208 |
// send packet to call for swarm on object
|
209 |
wl_send_global_packet(SWARM_GROUP,SWARM_ACTION, |
210 |
NULL,0,0); |
211 |
move(0,0); // stop |
212 |
break; // break loop |
213 |
} |
214 |
} |
215 |
|
216 |
// object manip goes here
|
217 |
orb_set_color(YELLOW); |
218 |
while(1) { |
219 |
wl_do(); |
220 |
} |
221 |
|
222 |
return 0; |
223 |
} |
224 |
|
225 |
|
226 |
void swarm_receive (char type, int source, unsigned char* packet, |
227 |
int length)
|
228 |
{ |
229 |
switch (type)
|
230 |
{ |
231 |
case SWARM_ACTION:
|
232 |
// do swarm action
|
233 |
bfs_init(source); // set robot_id to find
|
234 |
while(1) { |
235 |
bfs_fsm(); // do bfs
|
236 |
} |
237 |
} |
238 |
} |