Revision 591
BFS worked in 2 out of many trials with 2 robots. However, it dropped wireless a lot and changes states more than it should.
| bfs_fsm.c | ||
|---|---|---|
| 1 | 1 |
#include <dragonfly_lib.h> |
| 2 | 2 |
#include <wireless.h> |
| 3 | 3 |
#include <wl_token_ring.h> |
| 4 |
#include <queue.h>
|
|
| 4 |
#include "queue.h"
|
|
| 5 | 5 |
#include <string.h> |
| 6 | 6 |
#include "bfs_fsm.h" |
| 7 | 7 |
#include "smart_run_around_fsm.h" |
| ... | ... | |
| 17 | 17 |
|
| 18 | 18 |
|
| 19 | 19 |
/* private function prototypes */ |
| 20 |
void evaluate_state(void); |
|
| 20 |
void bfs_evaluate_state(void);
|
|
| 21 | 21 |
int bfs_follow(void); |
| 22 | 22 |
|
| 23 | 23 |
|
| ... | ... | |
| 40 | 40 |
|
| 41 | 41 |
bfs_pControl=0; |
| 42 | 42 |
bfs_bom = 0; |
| 43 |
bfs_bom_count = 0; |
|
| 43 | 44 |
|
| 44 | 45 |
|
| 45 | 46 |
/*Initialize distances to zero.*/ |
| ... | ... | |
| 57 | 58 |
wl_do(); // update wireless |
| 58 | 59 |
|
| 59 | 60 |
|
| 60 |
if (bfs_state == BFS_SEEK || bfs_state == BFS_FOLLOW) {
|
|
| 61 |
if (bfs_state == BFS_SEEK) {
|
|
| 61 | 62 |
bfs_follow_id = bfs_follow(); |
| 62 |
if (bfs_follow_id == BFS_NO_VAL) |
|
| 63 |
bfs_state = BFS_SEEK; // move to seek state |
|
| 63 |
if (bfs_follow_id != BFS_NO_VAL && bfs_follow_id < BFS_MAX_ROBOTS) |
|
| 64 |
bfs_state = BFS_FOLLOW; // move to follow state |
|
| 65 |
} |
|
| 66 |
if (bfs_state == BFS_FOLLOW) {
|
|
| 67 |
// get bom reading |
|
| 68 |
temp = wl_token_get_my_sensor_reading(bfs_follow_id); |
|
| 69 |
if (temp == -1) {
|
|
| 70 |
bfs_bom_count++; |
|
| 71 |
if (bfs_bom_count > BFS_MAX_BOM_COUNT) {
|
|
| 72 |
bfs_state = BFS_SEEK; |
|
| 73 |
bfs_bom = 255; |
|
| 74 |
bfs_bom_count = 0; |
|
| 75 |
} |
|
| 76 |
} |
|
| 64 | 77 |
else {
|
| 65 |
// found robot to follow to goal |
|
| 66 |
bfs_state = BFS_FOLLOW; |
|
| 67 |
|
|
| 68 |
// get bom reading |
|
| 69 |
temp = wl_token_get_my_sensor_reading(bfs_follow_id); |
|
| 70 |
bfs_bom = (temp == -1) ? bfs_bom : temp; |
|
| 71 |
|
|
| 78 |
bfs_bom = temp; |
|
| 79 |
|
|
| 72 | 80 |
// modify bom reading so right is negative, left is positive |
| 73 | 81 |
if (bfs_bom <= 12) |
| 74 | 82 |
bfs_bom -= 4; |
| ... | ... | |
| 99 | 107 |
} |
| 100 | 108 |
|
| 101 | 109 |
// evaluate state |
| 102 |
evaluate_state(); |
|
| 110 |
bfs_evaluate_state();
|
|
| 103 | 111 |
} |
| 104 | 112 |
|
| 105 | 113 |
|
| 106 | 114 |
//Acts on state change. |
| 107 |
void evaluate_state(){
|
|
| 115 |
void bfs_evaluate_state(){
|
|
| 108 | 116 |
switch(bfs_state){
|
| 109 | 117 |
case(BFS_SEEK): orb_set_color(RED); |
| 110 | 118 |
// move around |
| ... | ... | |
| 187 | 195 |
memset(node_distances, large_number, BFS_MAX_ROBOTS); |
| 188 | 196 |
|
| 189 | 197 |
//set the path to all LARGE_NUMBER as well |
| 190 |
memset(path, large_number, BFS_MAX_ROBOTS);
|
|
| 198 |
memset(path, BFS_NO_VAL, BFS_MAX_ROBOTS);
|
|
| 191 | 199 |
|
| 192 | 200 |
//queue_remove_all(q); |
| 193 | 201 |
|
Also available in: Unified diff