Revision 1627
More modifications to circle.c
trunk/code/behaviors/formation_control/circle/circle.c | ||
---|---|---|
33 | 33 |
if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15; |
34 | 34 |
} |
35 | 35 |
|
36 |
void switch_sending ()
|
|
36 |
void switch_sending(void)
|
|
37 | 37 |
{ |
38 | 38 |
if(sending) |
39 | 39 |
{ |
... | ... | |
126 | 126 |
|
127 | 127 |
/*~~~~~~~~~~~~~ NEED document |
128 | 128 |
*/ |
129 |
int correctTurn(void)
|
|
129 |
void correctTurn(void)
|
|
130 | 130 |
{ |
131 |
int bomNum = 0; |
|
132 |
bom_refresh(BOM_ALL); |
|
133 |
bomNum = bom_get_max(); |
|
134 |
usb_puti(bomNum); |
|
135 |
if(bomNum == 4) |
|
136 |
{ timeout = 0; |
|
137 |
//motor_l_set(1, 200); |
|
138 |
//motor_r_set(1, 200); |
|
139 |
return 1; //<--------------------------------------------1 and 0 are switched |
|
140 |
} |
|
141 |
else |
|
131 |
orb1_set_color(BLUE); // BLUE and PURPLE |
|
132 |
left(220); |
|
133 |
while(1) |
|
142 | 134 |
{ |
143 |
if(bomNum == -1){ |
|
144 |
timeout++; |
|
145 |
if(timeout > 500000) |
|
146 |
{ |
|
147 |
motor_r_set(FORWARD, 210); |
|
148 |
motor_l_set(BACKWARD, 190); |
|
149 |
} |
|
150 |
} |
|
151 |
else if((bomNum >= 12) || (bomNum < 4)) |
|
135 |
int bomNum = 0; // bomNum is the current maximum reading |
|
136 |
bom_refresh(BOM_ALL); |
|
137 |
bomNum = bom_get_max(); |
|
138 |
usb_puti(bomNum); |
|
139 |
if(bomNum == 4) // when it's turned the right way, stop |
|
152 | 140 |
{ |
153 |
motor_l_set(FORWARD, 200);timeout = 0; |
|
154 |
motor_r_set(BACKWARD, 200); |
|
141 |
timeout = 0; |
|
142 |
//motor_l_set(1, 200); |
|
143 |
//motor_r_set(1, 200); |
|
144 |
break; // exits the while() loop to stop the method |
|
155 | 145 |
} |
156 |
else |
|
146 |
else // facing the wrong way
|
|
157 | 147 |
{ |
158 |
motor_l_set(BACKWARD, 200);timeout = 0; |
|
159 |
motor_r_set(FORWARD, 200); |
|
148 |
if(bomNum == -1){ |
|
149 |
|
|
150 |
timeout++; |
|
151 |
|
|
152 |
if(timeout > 500000) // if it's been looking too long, move a little bit as it turns |
|
153 |
{ |
|
154 |
motor_r_set(FORWARD, 210); |
|
155 |
motor_l_set(BACKWARD, 190); |
|
156 |
} |
|
157 |
} |
|
158 |
else if((bomNum >= 12) || (bomNum < 4)) |
|
159 |
{ |
|
160 |
motor_l_set(FORWARD, 200); |
|
161 |
motor_r_set(BACKWARD, 200); |
|
162 |
timeout = 0; |
|
163 |
} |
|
164 |
else |
|
165 |
{ |
|
166 |
motor_l_set(BACKWARD, 200); |
|
167 |
motor_r_set(FORWARD, 200); |
|
168 |
timeout = 0; |
|
169 |
} |
|
160 | 170 |
} |
161 | 171 |
} |
162 |
return 0;
|
|
172 |
return; |
|
163 | 173 |
} |
164 | 174 |
|
165 | 175 |
|
166 | 176 |
/*~~~~~~~~~~~~~~ NEED document |
177 |
|
|
178 |
Actually, we should just get rid of this. |
|
167 | 179 |
*/ |
168 | 180 |
void correctApproach(void) |
169 | 181 |
{ |
... | ... | |
194 | 206 |
} |
195 | 207 |
|
196 | 208 |
|
197 |
/* |
|
198 |
Start turning until the front receive the MAx bom reading |
|
199 |
*/ |
|
200 |
void turn_to_beacon(int max){ |
|
201 |
if (max>-1 && max<16){ |
|
202 |
int index = (max+12)%16; |
|
203 |
if (index==0) { |
|
204 |
stop(); |
|
205 |
} |
|
206 |
else if (index<8) right(170); |
|
207 |
else left(170); |
|
208 |
} |
|
209 |
} |
|
210 |
|
|
211 |
|
|
212 |
/* |
|
213 |
Start turning its fron to the MAx bom reading |
|
214 |
But it won't stop by itself. |
|
215 |
*/ |
|
216 |
void turn_to_beacon2(int max){ // like the previous but no stop() call' |
|
217 |
|
|
218 |
|
|
219 |
|
|
220 |
|
|
221 |
if (max>-1 && max<16){ |
|
222 |
int index = (max+12)%16; |
|
223 |
if (index==0) { |
|
224 |
|
|
225 |
} |
|
226 |
else if (index<8) right(170); |
|
227 |
else left(170); |
|
228 |
} |
|
229 |
} |
|
230 |
|
|
231 |
|
|
232 |
|
|
233 |
|
|
234 |
/* |
|
235 |
Turn towards the MAX bom direction |
|
236 |
until the front receive the MAx bom reading |
|
237 |
*/ |
|
238 |
void orient(void){ |
|
239 |
int max_index = -1; |
|
240 |
while (max_index!=4) { |
|
241 |
/* Refresh and make sure the table is updated */ |
|
242 |
bom_refresh(BOM_ALL); |
|
243 |
max_index = bom_get_max(); |
|
244 |
turn_to_beacon(max_index); |
|
245 |
delay_ms(22); |
|
246 |
} |
|
247 |
} |
|
248 |
|
|
249 |
|
|
250 |
/* |
|
251 |
Turn towards the MAX bom direction |
|
252 |
until the front receive the MAx bom reading |
|
253 |
|
|
254 |
This function has less stop() so the robots run more smooth |
|
255 |
*/ |
|
256 |
void orient2(void){ |
|
257 |
int max_index = -1; |
|
258 |
while (max_index!=4) { |
|
259 |
/* Refresh and make sure the table is updated */ |
|
260 |
bom_refresh(BOM_ALL); |
|
261 |
max_index = bom_get_max(); |
|
262 |
turn_to_beacon2(max_index); |
|
263 |
delay_ms(22); |
|
264 |
} |
|
265 |
} |
|
266 | 209 |
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
267 | 210 |
void go_straight(void){ // drives forward a hardcoded distance. May not be useful. |
268 | 211 |
forward(200); |
... | ... | |
328 | 271 |
int beacon_State=0; |
329 | 272 |
int waitingCounter=0; |
330 | 273 |
int robotsReceived=0; |
274 |
|
|
331 | 275 |
if(wheel()<100) |
332 | 276 |
{ |
333 | 277 |
state=EDGE; |
... | ... | |
410 | 354 |
// COLOR afer DONE ---> MAGENTA |
411 | 355 |
orb_set_color(MAGENTA); |
412 | 356 |
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
413 |
while(correctTurn()){ |
|
414 |
} |
|
357 |
correctTurn(); // turn to face the beacon |
|
415 | 358 |
forward(220); |
416 | 359 |
//range_init(); |
417 | 360 |
|
... | ... | |
534 | 477 |
packet_data=wl_basic_do_default(&data_length); |
535 | 478 |
if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
536 | 479 |
{ |
537 |
orb_set_color(RED);orb2_set_color(BLUE); |
|
480 |
orb_set_color(RED); |
|
481 |
orb2_set_color(BLUE); |
|
538 | 482 |
//only add to robots seen if you haven't gotten an ACK from this robot |
539 | 483 |
if(used[packet_data[1]]==0){ |
540 | 484 |
robotsReceived++; |
... | ... | |
550 | 494 |
Sending DONE package. |
551 | 495 |
*/ |
552 | 496 |
case 3: |
497 |
blink(robotsReceived); |
|
553 | 498 |
orb_set_color(GREEN); |
554 | 499 |
send_buffer[0]=CIRCLE_ACTION_DONE; |
555 | 500 |
wl_basic_send_global_packet(42,send_buffer,2); |
trunk/code/behaviors/formation_control/circle/Makefile | ||
---|---|---|
11 | 11 |
USE_WIRELESS = 1 |
12 | 12 |
|
13 | 13 |
# com1 = serial port. Use lpt1 to connect to parallel port. |
14 |
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi) |
|
14 |
AVRDUDE_PORT = COM4 |
|
15 |
#AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi) |
|
15 | 16 |
else |
16 | 17 |
COLONYROOT := ../$(COLONYROOT) |
17 | 18 |
endif |
Also available in: Unified diff