Revision 750
object detection has been integrated. can distinguish green object from wall and swarm said object. has issues keeping the token ring going, but wireless packet transmission is fine.
driver.c | ||
---|---|---|
2 | 2 |
execute bfs behavior |
3 | 3 |
*/ |
4 | 4 |
|
5 |
#define COLOR_DETECT // comment out if color detect hardware is unavailable |
|
6 |
|
|
5 | 7 |
#include <dragonfly_lib.h> |
6 | 8 |
#include <wireless.h> |
7 | 9 |
#include <wl_token_ring.h> |
8 | 10 |
#include <dio.h> |
11 |
#include <analog.h> |
|
9 | 12 |
#include "bfs_fsm.h" |
10 | 13 |
#include "smart_run_around_fsm.h" |
11 | 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 |
|
|
12 | 29 |
#define SWARM_GROUP 15 |
13 | 30 |
#define SWARM_ACTION 16 |
14 | 31 |
|
... | ... | |
38 | 55 |
|
39 | 56 |
usb_puts("start\n\r"); |
40 | 57 |
|
58 |
|
|
59 |
#ifdef COLOR_DETECT |
|
60 |
/* set up color detect */ |
|
61 |
int rgb[3] = {0,0,0}; |
|
62 |
|
|
63 |
//long int distance_red; |
|
64 |
//long int distance_green; |
|
65 |
//long int distance_blue; |
|
66 |
|
|
67 |
int ratio_gb; |
|
68 |
|
|
69 |
//int blue[3] = {255,245,249}; |
|
70 |
//int red[3] = {190,122,126}; |
|
71 |
//int green[3] = {118, 80, 86}; |
|
72 |
|
|
73 |
analog_init(ADC_START); |
|
74 |
|
|
75 |
#else |
|
76 |
|
|
41 | 77 |
run_around_init(); |
42 | 78 |
|
79 |
#endif |
|
80 |
|
|
81 |
int object = 0; |
|
82 |
|
|
83 |
|
|
84 |
|
|
43 | 85 |
while(1) { |
44 | 86 |
wl_do(); // do wireless |
45 | 87 |
|
88 |
#ifdef COLOR_DETECT |
|
89 |
// need color sensor code here |
|
90 |
digital_output(BLUE_LED, 1); |
|
91 |
|
|
92 |
delay_ms(5); |
|
93 |
analog_stop_loop(); |
|
94 |
rgb[2] = analog8(AN9); |
|
95 |
analog_start_loop(); |
|
96 |
|
|
97 |
digital_output(BLUE_LED,0); |
|
98 |
digital_output(RED_LED, 1); |
|
99 |
delay_ms(5); |
|
100 |
|
|
101 |
analog_stop_loop(); |
|
102 |
rgb[0] = analog8(AN9); |
|
103 |
analog_start_loop(); |
|
104 |
|
|
105 |
digital_output(RED_LED, 0); |
|
106 |
digital_output(GREEN_LED,1); |
|
107 |
delay_ms(5); |
|
108 |
|
|
109 |
analog_stop_loop(); |
|
110 |
rgb[1] = analog8(AN9); |
|
111 |
analog_start_loop(); |
|
112 |
|
|
113 |
digital_output(GREEN_LED,0); |
|
114 |
|
|
115 |
ratio_gb = rgb[1] - rgb[2]; |
|
116 |
if (ratio_gb < -20) |
|
117 |
ratio_gb = 2; |
|
118 |
else if (ratio_gb > 20) |
|
119 |
ratio_gb = 0; |
|
120 |
else |
|
121 |
ratio_gb = 1; |
|
122 |
|
|
123 |
if (rgb[0] > 200 && rgb[1] > 200 && rgb[2] > 200) { |
|
124 |
// we think this is open area |
|
125 |
move(170,0); |
|
126 |
} else if (ratio_gb == COLOR_WALL) { |
|
127 |
// we think this is a wall, so turn away |
|
128 |
move(170,90); |
|
129 |
delay_ms(400); |
|
130 |
move(170,0); |
|
131 |
} else if (ratio_gb == COLOR_GREEN) { |
|
132 |
// we think this is a green object, so swarm |
|
133 |
object = 1; |
|
134 |
move(0,0); |
|
135 |
} |
|
136 |
else |
|
137 |
move(170,0); // just go forth |
|
138 |
|
|
139 |
/*distance_red = 1; |
|
140 |
distance_green = rgb[1] - rgb[0]; |
|
141 |
distance_blue = rgb[2] - rgb[0]; |
|
142 |
|
|
143 |
distance_blue = distance_blue * rgb[0]/255; |
|
144 |
distance_green = distance_green * rgb[0]/255;*/ |
|
145 |
|
|
146 |
usb_puts("R:"); |
|
147 |
usb_puti(rgb[0]); |
|
148 |
usb_puts("\tG:"); |
|
149 |
usb_puti(rgb[1]); |
|
150 |
usb_puts("\tB:"); |
|
151 |
usb_puti(rgb[2]); |
|
152 |
usb_puts("\t\tratio_gb:"); |
|
153 |
usb_puti(ratio_gb); |
|
154 |
usb_puts("\r\n"); |
|
155 |
|
|
156 |
#else |
|
46 | 157 |
// do smart run around until find object |
47 | 158 |
run_around_FSM(); |
159 |
//move(170,0); |
|
160 |
#endif |
|
48 | 161 |
|
49 |
// need color sensor code here |
|
162 |
/*usb_puts("R:"); |
|
163 |
usb_puti(distance_red); |
|
164 |
usb_puts("\tG:"); |
|
165 |
usb_puti(distance_green); |
|
166 |
usb_puts("\tB:"); |
|
167 |
usb_puti(distance_blue); |
|
168 |
usb_puts("\r\n");*/ |
|
50 | 169 |
|
51 |
if (button2_read()) { // if we find the object |
|
170 |
if (object || button2_read()) { // if we find the object
|
|
52 | 171 |
// send packet to call for swarm on object |
53 | 172 |
wl_send_global_packet(SWARM_GROUP,SWARM_ACTION, |
54 | 173 |
NULL,0,0); |
Also available in: Unified diff