root / trunk / code / projects / calibration_platform / robot / cal_sta_robot.c @ 1926
History | View | Annotate | Download (23.4 KB)
1 | 1926 | jsexton | |
---|---|---|---|
2 | /*** PROGRAM INFORMATION ***
|
||
3 | |||
4 | This program assembles a group of robots into a circle and allows the
|
||
5 | movement within that formation. Robots should be able to break formation and
|
||
6 | travel as a line, readjust in the face of obstacles, and reform if conditions
|
||
7 | are necessary.
|
||
8 | |||
9 | The program begins waiting for a button press. When pressed, a robot assumes
|
||
10 | the BEACON position, which means that it is the robot in the center of the
|
||
11 | circle and therefore in charge. It then gathers robots around it by sending
|
||
12 | them commands. This code is executed using two finite state machines, nested
|
||
13 | inside one another.
|
||
14 | One controls the overall state of the robot (whether it is a BEACON, an EDGE,
|
||
15 | or WAITING, for example).
|
||
16 | |||
17 | This code should be implemented so that most useful functions are built in
|
||
18 | to the machine. For example, the BEACON robot should be able to call methods
|
||
19 | such as CircleUp() to gather robots around it, and Move(distance) to move the
|
||
20 | circle group all at once.
|
||
21 | |||
22 | This Code is the property of the Carnegie Mellon Robotics Club and is being
|
||
23 | used to test formation control in a low-cost robot colony. Thanks to all
|
||
24 | members of RoboClub, especially Colony president John Sexton and graduade
|
||
25 | student representative Chris Mar.
|
||
26 | |||
27 | AUTHORS: James Carroll, Steve DeVincentis, Hanzhang (Echo) Hu, Nico Paris,
|
||
28 | Joel Rey, Reva Street, Alex Zirbel
|
||
29 | */
|
||
30 | |||
31 | |||
32 | #include <dragonfly_lib.h> |
||
33 | #include <wl_basic.h> |
||
34 | #include <encoders.h> |
||
35 | #include "cal_sta_robot.h" |
||
36 | |||
37 | /*** TODO: ***
|
||
38 | |||
39 | -Transform the code into a method-based state machine that uses the
|
||
40 | procedural state machines, which are hardcoded and hard to edit, as a backup.
|
||
41 | |||
42 | -Implement a drive straight method for use in keeping the robots more
|
||
43 | accurate as a group.
|
||
44 | |||
45 | -Fix the approach method: good robots usually work well, but bad robots often
|
||
46 | have errors which might be avoidable with the use of error checking.
|
||
47 | |||
48 | -Make robots more robust: packages are often lost, which throws the entire
|
||
49 | procedural nature of the program off.
|
||
50 | |||
51 | -Consider using the center bot to check distances
|
||
52 | |||
53 | -More testing is always good and necessary.
|
||
54 | */
|
||
55 | |||
56 | /*** BOT LOG ***
|
||
57 | |||
58 | 4-1-2010: BOT 7 as BEACON and BOT 1 as EDGE worked extremely well.
|
||
59 | 4-2-2010: BOT 7 and BOT 14 worked extremely well, no matter states. BOT 1
|
||
60 | started well, but malfunctioned later.
|
||
61 | */
|
||
62 | |||
63 | /*** TERMINOLOGY ***
|
||
64 | |||
65 | WAITINGSTATE:
|
||
66 | The robot waits to be given a signal to do something. Wireless is on,
|
||
67 | in case the robot is called on to turn into an EDGE. The color should be LIME
|
||
68 | or YELLOW-GREEN.
|
||
69 | |||
70 | BEACON_CONTROL:
|
||
71 | The code that executes commands when a robot is turned to BEACON mode. This
|
||
72 | code may run predefined methods for simplicity. One goal is to make these
|
||
73 | methods change the robot turn to to BEACON_MACHINE mode for a while, and then
|
||
74 | return to the CONTROL code where they left off.
|
||
75 | |||
76 | EDGE_CONTROL:
|
||
77 | Like BEACON_CONTROL, executes whatever orders are required of the robot as an
|
||
78 | EDGE.
|
||
79 | |||
80 | BEACON_MACHINE:
|
||
81 | A hardcoded list of functions which the robot is capable of running through.
|
||
82 | Consists of a finite state machine, where the robot executes a set of commands
|
||
83 | in a procedural manner and then returns to wherever it was in the control code.
|
||
84 | |||
85 | EDGE_MACHINE:
|
||
86 | Like the BEACON_MACHINE, but contains the same sort of procedural information
|
||
87 | for EDGE robots.
|
||
88 | |||
89 | END:
|
||
90 | A terminal state of the machine, where the robot just sits and waits. The
|
||
91 | color should be GREEN and WHITE.
|
||
92 | |||
93 | |||
94 | TYPES OF WIRELESS PACKETS:
|
||
95 | |||
96 | CIRCLE_ACTION_EXIST 'E'
|
||
97 | CIRCLE_ACTION_POSITION 'P'
|
||
98 | CIRCLE_ACTION_ACK 'A'
|
||
99 | A general acknowledgement package.
|
||
100 | CIRCLE_ACTION_DONE 'D'
|
||
101 | Used by robots to tell when they have finished their action.
|
||
102 | CIRCLE_ACTION_GOTYOU 'G'
|
||
103 | Used by the BEACON to tell a robot when it has been checked off.
|
||
104 | At this point, the EDGE has been recognized. Used for times when
|
||
105 | all EDGE robots have to communicate to the center via the spam method.
|
||
106 | CIRCLE_ACTION_FORWARD 'F'
|
||
107 | The BEACON tells the rest of the robots to move forward.
|
||
108 | CIRCLE_CLAIM_CENTER 'C'
|
||
109 | Sent out by a robot when it takes over as BEACON.
|
||
110 | */
|
||
111 | |||
112 | |||
113 | /* Define some variables to keep track of the state machine.*/
|
||
114 | int END = 100; |
||
115 | int WAITINGSTATE = 0; |
||
116 | int EDGE_CONTROL = 1; |
||
117 | int BEACON_CONTROL = 2; |
||
118 | int EDGE_MACHINE = 3; |
||
119 | int BEACON_MACHINE = 4; |
||
120 | |||
121 | int COUNT = 0; |
||
122 | int CIRCLEUP = 1; |
||
123 | int ORIENT = 2; |
||
124 | int DRIVE = 3; |
||
125 | int TURNL = 4; |
||
126 | int TURNR = 5; |
||
127 | |||
128 | int currentPos = 0; |
||
129 | int state = 0; |
||
130 | |||
131 | // keep track of the speed and duration of group movements.
|
||
132 | int speed = 20; |
||
133 | int duration = 2; |
||
134 | |||
135 | int timeout = 0; |
||
136 | int sending = 0; |
||
137 | int stop2 = 0; |
||
138 | struct vector slave_position;
|
||
139 | int desired_max_bom;
|
||
140 | int bom_max_counter;
|
||
141 | |||
142 | |||
143 | void switch_sending(void) |
||
144 | { |
||
145 | if(sending)
|
||
146 | { |
||
147 | sending = 0;
|
||
148 | bom_off(); |
||
149 | } |
||
150 | else
|
||
151 | { |
||
152 | sending = 1;
|
||
153 | bom_on(); |
||
154 | } |
||
155 | } |
||
156 | |||
157 | // set the motors to this forward speed.
|
||
158 | void forward(int speed) |
||
159 | { |
||
160 | motor_l_set(FORWARD,speed); |
||
161 | motor_r_set(FORWARD,speed); |
||
162 | } |
||
163 | // turn left at this speed.
|
||
164 | void left(int speed) |
||
165 | { |
||
166 | motor_l_set(BACKWARD,speed); |
||
167 | motor_r_set(FORWARD,speed); |
||
168 | } |
||
169 | void right(int speed) |
||
170 | { |
||
171 | motor_l_set(FORWARD,speed); |
||
172 | motor_r_set(BACKWARD,speed); |
||
173 | } |
||
174 | // stop() is better than motors_off(), which creates a slight delay when
|
||
175 | // reactivating the motors. Stop() is faster.
|
||
176 | void stop(void) |
||
177 | { |
||
178 | motor_l_set(BACKWARD,0);
|
||
179 | motor_r_set(FORWARD,0);
|
||
180 | } |
||
181 | void setforward(int spd1, int spd2) |
||
182 | { |
||
183 | motor_l_set(FORWARD,spd1); |
||
184 | motor_r_set(FORWARD,spd2); |
||
185 | } |
||
186 | void backward(int speed) |
||
187 | { |
||
188 | motor_l_set(BACKWARD, speed); |
||
189 | motor_r_set(BACKWARD, speed); |
||
190 | } |
||
191 | // takes an averaged reading of the front rangefinder
|
||
192 | int get_distance(void) |
||
193 | { |
||
194 | // kk sets this to 5 readings.
|
||
195 | int temp,distance,kk=5; |
||
196 | distance =0;
|
||
197 | for (int i=0; i<kk; i++) |
||
198 | { |
||
199 | temp = range_read_distance(IR2); |
||
200 | if (temp == -1) |
||
201 | { |
||
202 | //temp=0;
|
||
203 | i--; |
||
204 | } |
||
205 | else
|
||
206 | distance+= temp; |
||
207 | delay_ms(3);
|
||
208 | } |
||
209 | if (kk>0) |
||
210 | return (int)(distance/kk); |
||
211 | else
|
||
212 | return 0; |
||
213 | } |
||
214 | |||
215 | /* Sends a global packet with two arguments */
|
||
216 | void send2(char arg0, char arg1) |
||
217 | { |
||
218 | char send_buffer[2]; |
||
219 | send_buffer[0]=arg0;
|
||
220 | send_buffer[1]=arg1;
|
||
221 | wl_basic_send_global_packet(42,send_buffer,2); |
||
222 | } |
||
223 | |||
224 | /* Sends a global packet with three arguments */
|
||
225 | void send3(char arg0, char arg1, char arg2) |
||
226 | { |
||
227 | char send_buffer[3]; |
||
228 | send_buffer[0]=arg0;
|
||
229 | send_buffer[1]=arg1;
|
||
230 | send_buffer[2]=arg2;
|
||
231 | wl_basic_send_global_packet(42,send_buffer,3); |
||
232 | } |
||
233 | |||
234 | /*
|
||
235 | Orients the robot so that it is facing the beacon (or the broadcasting BOM).
|
||
236 | */
|
||
237 | void faceFront(void) |
||
238 | { |
||
239 | int counter = 0; |
||
240 | int currentDir = 0; |
||
241 | left(200);
|
||
242 | int bomNum = -1; |
||
243 | orb1_set_color(BLUE); |
||
244 | while(bomNum != 4) |
||
245 | { |
||
246 | if(counter >= 5) |
||
247 | { |
||
248 | forward(200);
|
||
249 | delay_ms(750);
|
||
250 | counter = 0;
|
||
251 | } |
||
252 | bom_refresh(BOM_ALL); |
||
253 | bomNum = bom_get_max(); |
||
254 | if(bomNum == -1) |
||
255 | { |
||
256 | //ignore
|
||
257 | } |
||
258 | else if((bomNum < 4) || (bomNum >= 12)) |
||
259 | { |
||
260 | right(200);
|
||
261 | if(currentDir == 0) |
||
262 | counter++; |
||
263 | currentDir = 1;
|
||
264 | } |
||
265 | else
|
||
266 | { |
||
267 | left(200);
|
||
268 | if(currentDir == 1) |
||
269 | counter++; |
||
270 | currentDir = 0;
|
||
271 | } |
||
272 | } |
||
273 | stop(); |
||
274 | return;
|
||
275 | } |
||
276 | |||
277 | /*
|
||
278 | Turns the robot slowly to the right until it reaches the BOM reading goal.
|
||
279 | More stable code than what was implemented ealier, with smart turning,
|
||
280 | but slower.
|
||
281 | */
|
||
282 | void aboutFace(int goal) |
||
283 | { |
||
284 | int bomNum = -1; |
||
285 | int speed = 170; // speed with which to turn |
||
286 | |||
287 | orb1_set_color(BLUE); // BLUE and PURPLE
|
||
288 | |||
289 | while(bomNum != goal)
|
||
290 | { |
||
291 | // bomNum is the current maximum reading
|
||
292 | bom_refresh(BOM_ALL); |
||
293 | bomNum = bom_get_max(); |
||
294 | right(speed); |
||
295 | } |
||
296 | stop(); |
||
297 | return;
|
||
298 | } |
||
299 | |||
300 | |||
301 | /*
|
||
302 | BLINK the given number times
|
||
303 | */
|
||
304 | void blink(int num) |
||
305 | { |
||
306 | for(int i = 0; i<num; i++) |
||
307 | { |
||
308 | orb_set_color(ORB_OFF); |
||
309 | delay_ms(150);
|
||
310 | orb_set_color(RED); |
||
311 | delay_ms(50);
|
||
312 | } |
||
313 | orb_set_color(ORB_OFF); |
||
314 | } |
||
315 | |||
316 | /*
|
||
317 | BLINK slowly the given number times
|
||
318 | */
|
||
319 | void slowblink(int num) |
||
320 | { |
||
321 | for(int i = 0; i<num; i++) |
||
322 | { |
||
323 | orb_set_color(ORB_OFF); |
||
324 | delay_ms(300);
|
||
325 | orb_set_color(RED); |
||
326 | delay_ms(200);
|
||
327 | } |
||
328 | orb_set_color(ORB_OFF); |
||
329 | } |
||
330 | |||
331 | /*
|
||
332 | A method for the higher-level code for the BEACON. The beacon can make
|
||
333 | any of the preprogrammed commands, and this code sends the packet and
|
||
334 | transitions the robots correctly.
|
||
335 | */
|
||
336 | void order(int action) |
||
337 | { |
||
338 | currentPos++; |
||
339 | send2(CIRCLE_EXECUTE, action); |
||
340 | state = 20 + action;
|
||
341 | } |
||
342 | |||
343 | /*
|
||
344 | A method for the higher-level code for the BEACON. The beacond sends
|
||
345 | not only the command, but also the speed and duration for which the
|
||
346 | (movement) command is to be executed.
|
||
347 | */
|
||
348 | void orderMove(int action, int newSpeed, int newDuration) |
||
349 | { |
||
350 | currentPos++; |
||
351 | speed = newSpeed; |
||
352 | duration = newDuration; |
||
353 | send2(CIRCLE_EXECUTE, action); |
||
354 | state = 20 + action;
|
||
355 | } |
||
356 | |||
357 | /*
|
||
358 | Turns off the motors, sends an EXECUTE packet, and blinks green and white
|
||
359 | forever.
|
||
360 | */
|
||
361 | void terminate(void) |
||
362 | { |
||
363 | motors_off(); |
||
364 | send2(CIRCLE_EXECUTE, 100);
|
||
365 | orb_set_color(GREEN); |
||
366 | orb2_set_color(WHITE); |
||
367 | while(1) ; |
||
368 | } |
||
369 | |||
370 | |||
371 | //******************************************************************************
|
||
372 | //******************************************************************************
|
||
373 | //******************************************************************************
|
||
374 | |||
375 | |||
376 | |||
377 | /*
|
||
378 | A state machine with five states. The robot starts out in WAITINGSTATE mode,
|
||
379 | from which it recieves a signal of some sort and moves to a different state.
|
||
380 | */
|
||
381 | int main(void) |
||
382 | { |
||
383 | /* Initialize dragonfly board */
|
||
384 | dragonfly_init(ALL_ON); |
||
385 | /* Initialize the basic wireless library */
|
||
386 | wl_basic_init_default(); |
||
387 | /* Set the XBee channel to 24 - must be standard among robots */
|
||
388 | wl_set_channel(24);
|
||
389 | |||
390 | int robotid = get_robotid();
|
||
391 | |||
392 | // once the EDGE gets the first signal from a center, it stores who the // center is.
|
||
393 | int centerid = 0; |
||
394 | |||
395 | // stores a list of bots which are in the group by storing a "1" in the
|
||
396 | // array if the robot of that index is in the group.
|
||
397 | int used[17]; |
||
398 | int numOk;
|
||
399 | |||
400 | // initially, no robots in the group.
|
||
401 | for (int i=0; i<17; i++) |
||
402 | used[i] = 0;
|
||
403 | |||
404 | // keeps track of the length of wireless packets received.
|
||
405 | int data_length;
|
||
406 | unsigned char *packet_data=wl_basic_do_default(&data_length); |
||
407 | |||
408 | // these variables keep track of the inner state machines in the
|
||
409 | // procedural MACHINE states.
|
||
410 | int beacon_State=0; |
||
411 | int edge_State=0; |
||
412 | |||
413 | int waitingCounter=0; |
||
414 | |||
415 | // an important variable that stores the size of the group.
|
||
416 | int robotsReceived=0; |
||
417 | |||
418 | // offset for the approaching: how far off the rangefinders can be
|
||
419 | int offset = 20; |
||
420 | int time=0; |
||
421 | |||
422 | // keeps track of which way robots are facing relative to the center
|
||
423 | int direction = 4; |
||
424 | |||
425 | // how far away the robot is. Initialized to a large value to ensure
|
||
426 | // that the robot doesn't think it is already the right distance away.
|
||
427 | int distance=1000; |
||
428 | int onefoot = 250; // how far away to stop. |
||
429 | |||
430 | while(1) |
||
431 | { |
||
432 | bom_refresh(BOM_ALL); |
||
433 | |||
434 | /***EXPECTED MOVES***
|
||
435 | (OUT OF DATE. Will be updated once changes have been made.)
|
||
436 | The designed movement:
|
||
437 | 1. one center robot, several edge robots are on;
|
||
438 | 2. center robots: button 1 is pressed;
|
||
439 | 3. center robots: send global package telling edges that he exists;
|
||
440 | 4. EDGE robots response with ACK.
|
||
441 | 5. EDGE robots wait for center robots to finish counting (DONE package)
|
||
442 | 6. EDGE robtos approach the center robtot and stop at the "onefoot"
|
||
443 | distance, send message to the center
|
||
444 | */
|
||
445 | |||
446 | |||
447 | /*
|
||
448 | This is the MAIN SWITCH LOOP, which governs the overall
|
||
449 | status of the robot.
|
||
450 | */
|
||
451 | switch(state)
|
||
452 | { |
||
453 | |||
454 | |||
455 | /*
|
||
456 | The WAITINGSTATE. This state constantly checks for wireless
|
||
457 | packets,
|
||
458 | and updates its state as soon as it receives a signal.
|
||
459 | */
|
||
460 | case 0: |
||
461 | |||
462 | orb_set_color(YELLOW); |
||
463 | packet_data=wl_basic_do_default(&data_length); |
||
464 | if(packet_data != 0 && data_length>=2 |
||
465 | && packet_data[0]==CIRCLE_CLAIM_CENTER)
|
||
466 | { |
||
467 | centerid = packet_data[1];
|
||
468 | state = 1;
|
||
469 | } |
||
470 | |||
471 | if(button1_read())
|
||
472 | { |
||
473 | // becomes the center if button1 is clicked.
|
||
474 | send2(CIRCLE_CLAIM_CENTER, robotid); |
||
475 | state = 2;
|
||
476 | } |
||
477 | break;
|
||
478 | |||
479 | |||
480 | |||
481 | //******************************************************************************
|
||
482 | //******************************************************************************
|
||
483 | |||
484 | |||
485 | /*
|
||
486 | The CONTROL for the EDGE state. This sets a certain procedure
|
||
487 | to follow, in the form of simple
|
||
488 | commands, for a robot to follow if it is set to an EDGE.
|
||
489 | */
|
||
490 | |||
491 | case 1: |
||
492 | orb_set_color(CYAN); |
||
493 | orb1_set_color(YELLOW); |
||
494 | |||
495 | int command = -1; |
||
496 | |||
497 | packet_data=wl_basic_do_default(&data_length); |
||
498 | |||
499 | if(packet_data != 0 && data_length>=2 && |
||
500 | packet_data[0]==CIRCLE_EXECUTE)
|
||
501 | { |
||
502 | command = packet_data[1];
|
||
503 | } |
||
504 | |||
505 | if(command != -1) |
||
506 | { |
||
507 | edge_State = 0;
|
||
508 | switch(command)
|
||
509 | { |
||
510 | case 0: |
||
511 | state = 10; break; |
||
512 | |||
513 | case 1: |
||
514 | state = 11; break; |
||
515 | |||
516 | case 2: |
||
517 | state = 12; break; |
||
518 | |||
519 | case 3: |
||
520 | state = 13; break; |
||
521 | |||
522 | case 4: |
||
523 | state = 14; break; |
||
524 | |||
525 | case 5: |
||
526 | state = 15; break; |
||
527 | |||
528 | case 100: |
||
529 | terminate(); break;
|
||
530 | } |
||
531 | } |
||
532 | |||
533 | break;
|
||
534 | |||
535 | |||
536 | |||
537 | //******************************************************************************
|
||
538 | //******************************************************************************
|
||
539 | |||
540 | |||
541 | /*
|
||
542 | The CONTROL for the BEACON state. This sets a certain procedure
|
||
543 | to follow, in the form of simple commands, for a robot to follow
|
||
544 | if it is set to a BEACON.
|
||
545 | */
|
||
546 | case 2: |
||
547 | orb_set_color(PURPLE); |
||
548 | beacon_State = 0;
|
||
549 | |||
550 | switch(currentPos)
|
||
551 | { |
||
552 | case 0: |
||
553 | order(COUNT); break;
|
||
554 | |||
555 | case 1: |
||
556 | order(CIRCLEUP); break;
|
||
557 | |||
558 | case 2: |
||
559 | order(ORIENT); break;
|
||
560 | |||
561 | case 3: |
||
562 | orderMove(DRIVE,20,2); break; |
||
563 | |||
564 | case 4: |
||
565 | order(CIRCLEUP); break;
|
||
566 | |||
567 | case 5: |
||
568 | order(ORIENT); break;
|
||
569 | |||
570 | case 6: |
||
571 | orderMove(TURNR,18,1); break; |
||
572 | |||
573 | case 7: |
||
574 | orderMove(DRIVE,20,2); break; |
||
575 | |||
576 | case 8: |
||
577 | order(CIRCLEUP); break;
|
||
578 | |||
579 | case 9: |
||
580 | order(ORIENT); break;
|
||
581 | |||
582 | case 10: |
||
583 | terminate(); break;
|
||
584 | |||
585 | } |
||
586 | |||
587 | break;
|
||
588 | |||
589 | |||
590 | //******************************************************************************
|
||
591 | //******************************************************************************
|
||
592 | |||
593 | |||
594 | /* The following states are MACHINE states for the EDGE robot. */
|
||
595 | |||
596 | /*
|
||
597 | EDGE on COUNT
|
||
598 | */
|
||
599 | case 10: |
||
600 | |||
601 | switch(edge_State)
|
||
602 | { |
||
603 | /*
|
||
604 | 0. EDGE robots are on.
|
||
605 | 1. They are waiting for EXIST pacakage from the
|
||
606 | Center robots
|
||
607 | 2. After they receive the package, they send ACK
|
||
608 | package to center.
|
||
609 | 3. Done for now: display green.
|
||
610 | */
|
||
611 | case 0: |
||
612 | bom_off(); |
||
613 | orb1_set_color(YELLOW); |
||
614 | orb2_set_color(BLUE); |
||
615 | packet_data=wl_basic_do_default(&data_length); |
||
616 | |||
617 | if(packet_data != 0 && data_length>=2 && |
||
618 | packet_data[0]==CIRCLE_ACTION_EXIST)
|
||
619 | { |
||
620 | centerid = packet_data[1];
|
||
621 | |||
622 | send2(CIRCLE_ACTION_ACK,robotid); |
||
623 | |||
624 | edge_State=1;
|
||
625 | } |
||
626 | break;
|
||
627 | |||
628 | /*
|
||
629 | 1. Wait for DONE package
|
||
630 | 2. The counting process is DONE
|
||
631 | */
|
||
632 | case 1: |
||
633 | |||
634 | orb_set_color(YELLOW); |
||
635 | orb2_set_color(PURPLE); |
||
636 | |||
637 | // keep sending the packet until we get a
|
||
638 | // response
|
||
639 | send2(CIRCLE_ACTION_ACK,robotid); |
||
640 | |||
641 | packet_data=wl_basic_do_default(&data_length); |
||
642 | if(packet_data != 0 && data_length>=2 && |
||
643 | packet_data[0]==CIRCLE_ACTION_GOTYOU &&
|
||
644 | packet_data[1] == robotid)
|
||
645 | { |
||
646 | edge_State=2;
|
||
647 | } |
||
648 | break;
|
||
649 | |||
650 | // wait for the second, general, done packet.
|
||
651 | case 2: |
||
652 | |||
653 | orb_set_color(YELLOW); |
||
654 | packet_data=wl_basic_do_default(&data_length); |
||
655 | if(packet_data != 0 && data_length>=2 && |
||
656 | packet_data[0]==CIRCLE_ACTION_DONE &&
|
||
657 | packet_data[1] == centerid)
|
||
658 | { |
||
659 | state = 1;
|
||
660 | } |
||
661 | break;
|
||
662 | } |
||
663 | |||
664 | break;
|
||
665 | |||
666 | /* The CIRCLEUP command for EDGE */
|
||
667 | |||
668 | case 11: |
||
669 | |||
670 | switch(edge_State)
|
||
671 | { |
||
672 | |||
673 | case 0: |
||
674 | // COLOR afer DONE ---> MAGENTA
|
||
675 | orb_set_color(MAGENTA); |
||
676 | // turn to face the beacon
|
||
677 | faceFront(); |
||
678 | forward(175);
|
||
679 | //range_init();
|
||
680 | |||
681 | |||
682 | distance = get_distance(); |
||
683 | time=0;
|
||
684 | while ((distance-offset)>=onefoot ||
|
||
685 | distance==0 || (distance+offset)<onefoot)
|
||
686 | { |
||
687 | if(distance==0) |
||
688 | orb_set_color(WHITE); |
||
689 | else if(distance-offset>=onefoot) |
||
690 | forward(175);
|
||
691 | else
|
||
692 | backward(175);
|
||
693 | distance = get_distance(); |
||
694 | delay_ms(14);
|
||
695 | time+=14;
|
||
696 | if(time>30) |
||
697 | { |
||
698 | faceFront(); |
||
699 | time=0;
|
||
700 | } |
||
701 | } |
||
702 | |||
703 | stop(); |
||
704 | orb_set_color(GREEN); |
||
705 | |||
706 | send2(CIRCLE_ACTION_ACK, robotid); |
||
707 | |||
708 | stop(); |
||
709 | state = 1;
|
||
710 | break;
|
||
711 | } |
||
712 | |||
713 | |||
714 | break;
|
||
715 | |||
716 | /* An ORIENT series of steps for the EDGE robot. */
|
||
717 | |||
718 | case 12: |
||
719 | |||
720 | switch(edge_State)
|
||
721 | { |
||
722 | |||
723 | // waits for a packet to tell it to turn on the bom.
|
||
724 | case 0: |
||
725 | packet_data=wl_basic_do_default(&data_length); |
||
726 | if(packet_data != 0 && data_length==2 && |
||
727 | packet_data[0]==CIRCLE_ACTION_GOTYOU &&
|
||
728 | packet_data[1] == robotid)
|
||
729 | { |
||
730 | bom_on(); |
||
731 | orb_set_color(ORANGE); |
||
732 | send2(CIRCLE_ACTION_ACK,centerid); |
||
733 | edge_State = 1;
|
||
734 | } |
||
735 | break;
|
||
736 | |||
737 | // waits for a packet to tell it that it has been
|
||
738 | // received.
|
||
739 | case 1: |
||
740 | orb2_set_color(YELLOW); |
||
741 | packet_data=wl_basic_do_default(&data_length); |
||
742 | if(packet_data != 0 && data_length==3 && |
||
743 | packet_data[0]==CIRCLE_ACTION_GOTYOU &&
|
||
744 | packet_data[1] == robotid)
|
||
745 | { |
||
746 | bom_off(); |
||
747 | direction = packet_data[2];
|
||
748 | orb_set_color(YELLOW); |
||
749 | edge_State = 2;
|
||
750 | } |
||
751 | break;
|
||
752 | |||
753 | /*
|
||
754 | Wait for the center bot to send a DONE packet; then
|
||
755 | turn to face the right direction.
|
||
756 | */
|
||
757 | case 2: |
||
758 | orb_set_color(GREEN); |
||
759 | packet_data=wl_basic_do_default(&data_length); |
||
760 | if(packet_data != 0 && data_length>=2 && |
||
761 | packet_data[0]==CIRCLE_ACTION_DONE)
|
||
762 | { |
||
763 | orb_set_color(WHITE); |
||
764 | orb2_set_color(CYAN); |
||
765 | edge_State = 3;
|
||
766 | } |
||
767 | break;
|
||
768 | |||
769 | /* Turn until we reach the right direction */
|
||
770 | case 3: |
||
771 | aboutFace(direction); |
||
772 | stop(); |
||
773 | orb_set_color(YELLOW); |
||
774 | send2(CIRCLE_ACTION_DONE,robotid); |
||
775 | state = 1;
|
||
776 | break;
|
||
777 | |||
778 | } |
||
779 | |||
780 | break;
|
||
781 | |||
782 | |||
783 | /* The DRIVE steps for the EDGE robot */
|
||
784 | case 13: |
||
785 | |||
786 | /* Wait for specifications to drive */
|
||
787 | packet_data=wl_basic_do_default(&data_length); |
||
788 | if(packet_data != 0 && data_length>=3 && |
||
789 | packet_data[0]==CIRCLE_ACTION_FORWARD)
|
||
790 | { |
||
791 | orb_set_color(BLUE); |
||
792 | |||
793 | forward(packet_data[1]*10); |
||
794 | delay_ms(packet_data[2]*1000); |
||
795 | stop(); |
||
796 | state = 1;
|
||
797 | } |
||
798 | |||
799 | break;
|
||
800 | |||
801 | /* The TURNL steps for the EDGE robot */
|
||
802 | case 14: |
||
803 | |||
804 | /* Wait for specifications for the turn. */
|
||
805 | packet_data=wl_basic_do_default(&data_length); |
||
806 | if(packet_data != 0 && data_length>=3 && |
||
807 | packet_data[0]==CIRCLE_ACTION_TURN)
|
||
808 | { |
||
809 | orb_set_color(BLUE); |
||
810 | |||
811 | left(packet_data[1]*10); |
||
812 | delay_ms(packet_data[2]*1000); |
||
813 | stop(); |
||
814 | state = 1;
|
||
815 | } |
||
816 | break;
|
||
817 | |||
818 | /* The TURNR steps for the EDGE robot */
|
||
819 | case 15: |
||
820 | |||
821 | /* Wait for specifications for the turn. */
|
||
822 | packet_data=wl_basic_do_default(&data_length); |
||
823 | if(packet_data != 0 && data_length>=3 && |
||
824 | packet_data[0]==CIRCLE_ACTION_TURN)
|
||
825 | { |
||
826 | orb_set_color(BLUE); |
||
827 | |||
828 | right(packet_data[1]*10); |
||
829 | delay_ms(packet_data[2]*1000); |
||
830 | stop(); |
||
831 | state = 1;
|
||
832 | } |
||
833 | break;
|
||
834 | |||
835 | // END for EDGE robots
|
||
836 | |||
837 | |||
838 | |||
839 | //******************************************************************************
|
||
840 | //******************************************************************************
|
||
841 | |||
842 | |||
843 | /*
|
||
844 | The MACHINE for the BEACON state
|
||
845 | */
|
||
846 | |||
847 | /* the COUNT code for the BEACON */
|
||
848 | case 20: |
||
849 | switch(beacon_State)
|
||
850 | { |
||
851 | |||
852 | /* 0. center robots on wait for pressing button 1 */
|
||
853 | case 0: |
||
854 | bom_on(); |
||
855 | orb_set_color(BLUE); |
||
856 | robotsReceived = 0;
|
||
857 | beacon_State=1;
|
||
858 | break;
|
||
859 | |||
860 | /* 1. Send EXIST package to EDGE robots */
|
||
861 | case 1: |
||
862 | orb_set_color(RED); |
||
863 | send2(CIRCLE_ACTION_EXIST,robotid); |
||
864 | beacon_State=2;
|
||
865 | break;
|
||
866 | |||
867 | /* 2. Count the number of the EDGE robots
|
||
868 | *******NOTE: at most 1000 times of loop ****** */
|
||
869 | case 2: |
||
870 | waitingCounter++; |
||
871 | orb1_set_color(YELLOW); |
||
872 | orb2_set_color(BLUE); |
||
873 | packet_data=wl_basic_do_default(&data_length); |
||
874 | |||
875 | if(packet_data!=0 && data_length>=2 && |
||
876 | packet_data[0]==CIRCLE_ACTION_ACK)
|
||
877 | { |
||
878 | orb_set_color(RED); |
||
879 | orb2_set_color(BLUE); |
||
880 | // only add to list seen if you haven't
|
||
881 | // gotten an ACK from this robot
|
||
882 | if(used[packet_data[1]]==0) |
||
883 | { |
||
884 | robotsReceived++; |
||
885 | used[packet_data[1]] = 1; |
||
886 | |||
887 | usb_puts("Added: ");
|
||
888 | usb_puti(packet_data[1]);
|
||
889 | usb_puts("\r\n");
|
||
890 | } |
||
891 | |||
892 | // NEW: sends a packet to each robot it
|
||
893 | // receives telling them to be done.
|
||
894 | send2(CIRCLE_ACTION_GOTYOU, |
||
895 | packet_data[1]);
|
||
896 | } |
||
897 | if(waitingCounter >= 300) |
||
898 | { |
||
899 | beacon_State=3;
|
||
900 | } |
||
901 | break;
|
||
902 | |||
903 | /* COUNTing is DONE. Sending DONE package. */
|
||
904 | case 3: |
||
905 | blink(robotsReceived); |
||
906 | orb_set_color(GREEN); |
||
907 | send2(CIRCLE_ACTION_DONE, robotid); |
||
908 | state = 2;
|
||
909 | break;
|
||
910 | } |
||
911 | |||
912 | break;
|
||
913 | |||
914 | /* The CIRCLEUP method for BEACON */
|
||
915 | case 21: |
||
916 | |||
917 | switch(beacon_State)
|
||
918 | { |
||
919 | |||
920 | /* Wait for all the robots to get to right distance */
|
||
921 | case 0: |
||
922 | // left(170);
|
||
923 | orb1_set_color(YELLOW); |
||
924 | orb2_set_color(WHITE); |
||
925 | |||
926 | numOk = 0;
|
||
927 | |||
928 | while(numOk<robotsReceived)
|
||
929 | { |
||
930 | packet_data= |
||
931 | wl_basic_do_default(&data_length); |
||
932 | if(packet_data!=0 && data_length>=2 && |
||
933 | packet_data[0]==CIRCLE_ACTION_ACK)
|
||
934 | { |
||
935 | numOk++; |
||
936 | } |
||
937 | } |
||
938 | |||
939 | state = 2;
|
||
940 | break;
|
||
941 | } |
||
942 | |||
943 | break;
|
||
944 | |||
945 | |||
946 | /* The ORIENT code for the beacon */
|
||
947 | case 22: |
||
948 | |||
949 | switch(beacon_State)
|
||
950 | { |
||
951 | /* Turns all the robots in the same direction */
|
||
952 | case 0: |
||
953 | stop(); |
||
954 | bom_off(); |
||
955 | orb_set_color(ORANGE); |
||
956 | |||
957 | // for each robot, tells them to turn their bom
|
||
958 | // on, then tells them which way to face.
|
||
959 | for(int i=0; i < 17; i++) |
||
960 | { |
||
961 | if(used[i] == 1) |
||
962 | { |
||
963 | send2(CIRCLE_ACTION_GOTYOU, i); |
||
964 | // waits for a response so it knows the
|
||
965 | // BOM is on.
|
||
966 | while(1) |
||
967 | { |
||
968 | orb_set_color(RED); |
||
969 | orb2_set_color(WHITE); |
||
970 | packet_data=wl_basic_do_default( |
||
971 | &data_length); |
||
972 | if(packet_data!=0 && data_length>=2 |
||
973 | && packet_data[0]==
|
||
974 | CIRCLE_ACTION_ACK) |
||
975 | { |
||
976 | orb_set_color(ORANGE); |
||
977 | break;
|
||
978 | } |
||
979 | } |
||
980 | delay_ms(20);
|
||
981 | bom_refresh(BOM_ALL); |
||
982 | direction = bom_get_max(); |
||
983 | |||
984 | direction += 8;
|
||
985 | if(direction > 15) direction -= 16; |
||
986 | |||
987 | delay_ms(20);
|
||
988 | |||
989 | send3(CIRCLE_ACTION_GOTYOU, i, |
||
990 | direction); |
||
991 | |||
992 | delay_ms(20);
|
||
993 | } |
||
994 | } |
||
995 | beacon_State = 1;
|
||
996 | break;
|
||
997 | |||
998 | /*
|
||
999 | Sends a DONE packet to signify that it has read in all
|
||
1000 | the robots' directions and sent packets.
|
||
1001 | Edge robots should now turn to face the right direction.
|
||
1002 | */
|
||
1003 | case 1: |
||
1004 | send2(CIRCLE_ACTION_DONE,robotid); |
||
1005 | bom_on(); |
||
1006 | beacon_State = 2;
|
||
1007 | break;
|
||
1008 | |||
1009 | case 2: |
||
1010 | numOk = 0;
|
||
1011 | |||
1012 | while(numOk < robotsReceived)
|
||
1013 | { |
||
1014 | orb_set_color(ORANGE); |
||
1015 | packet_data=wl_basic_do_default( |
||
1016 | &data_length); |
||
1017 | |||
1018 | if(packet_data!=0 && data_length>=2 && |
||
1019 | packet_data[0]==CIRCLE_ACTION_DONE)
|
||
1020 | { |
||
1021 | numOk++; |
||
1022 | } |
||
1023 | } |
||
1024 | state = 2;
|
||
1025 | break;
|
||
1026 | } |
||
1027 | |||
1028 | break;
|
||
1029 | |||
1030 | |||
1031 | /* The DRIVE code for the beacon */
|
||
1032 | case 23: |
||
1033 | |||
1034 | orb_set_color(YELLOW); |
||
1035 | delay_ms(100);
|
||
1036 | |||
1037 | // format: type of ack, speed divided by 10,
|
||
1038 | // time in seconds.
|
||
1039 | for(int i = 0 ; i < 13; i++) |
||
1040 | send3(CIRCLE_ACTION_FORWARD,speed,duration); |
||
1041 | orb_set_color(BLUE); |
||
1042 | forward(speed*10);
|
||
1043 | delay_ms(duration*1000);
|
||
1044 | stop(); |
||
1045 | state = 2;
|
||
1046 | break;
|
||
1047 | |||
1048 | /* The TURNL code for the beacon */
|
||
1049 | case 24: |
||
1050 | |||
1051 | orb_set_color(YELLOW); |
||
1052 | delay_ms(100);
|
||
1053 | |||
1054 | // format: type of ack, speed divided by 10,
|
||
1055 | // time in seconds.
|
||
1056 | for(int i = 0 ; i < 13; i++) |
||
1057 | send3(CIRCLE_ACTION_TURN,speed,duration); |
||
1058 | orb_set_color(BLUE); |
||
1059 | left(speed*10);
|
||
1060 | delay_ms(duration*1000);
|
||
1061 | stop(); |
||
1062 | state = 2;
|
||
1063 | |||
1064 | break;
|
||
1065 | |||
1066 | /* The TURNR code for the beacon */
|
||
1067 | case 25: |
||
1068 | |||
1069 | orb_set_color(YELLOW); |
||
1070 | delay_ms(100);
|
||
1071 | |||
1072 | // format: type of ack, speed divided by 10,
|
||
1073 | // time in seconds.
|
||
1074 | for(int i = 0 ; i < 13; i++) |
||
1075 | send3(CIRCLE_ACTION_TURN,speed,duration); |
||
1076 | orb_set_color(BLUE); |
||
1077 | right(speed*10);
|
||
1078 | delay_ms(duration*1000);
|
||
1079 | stop(); |
||
1080 | state = 2;
|
||
1081 | |||
1082 | break;
|
||
1083 | |||
1084 | |||
1085 | //******************************************************************************
|
||
1086 | //******************************************************************************
|
||
1087 | |||
1088 | } // ends the main switch
|
||
1089 | } // ends the main while loop
|
||
1090 | |||
1091 | // error, we should never break from the while loop!
|
||
1092 | orb_set_color(RED); |
||
1093 | |||
1094 | /*
|
||
1095 | END HERE, just in case something happened.
|
||
1096 | This way we can see the red orb.
|
||
1097 | */
|
||
1098 | while(1); |
||
1099 | } |