Revision 711
added slowing feature to bfs as it approaches its target, and fixed some rangefinder distances.
added code to allow for better transitioning to seek the final robot in a multiple robot scenario, but needs testing.
| bfs_fsm.c | ||
|---|---|---|
| 9 | 9 |
|
| 10 | 10 |
/* Used to find a robot (or other object) |
| 11 | 11 |
|
| 12 |
Uses bom and token ring |
|
| 12 |
Uses bom and token ring and rangefinders
|
|
| 13 | 13 |
|
| 14 | 14 |
Assumes: |
| 15 | 15 |
robots are already in a token ring |
| ... | ... | |
| 50 | 50 |
|
| 51 | 51 |
/*The main function, call this to update states as frequently as possible.*/ |
| 52 | 52 |
void bfs_fsm(void) {
|
| 53 |
|
|
| 54 |
/* reset some values */ |
|
| 55 |
bfs_speed = BFS_STRAIGHT_SPEED; |
|
| 53 | 56 |
|
| 54 | 57 |
/*The following lines ensure that undefined (-1) values |
| 55 | 58 |
will not update the distances.*/ |
| ... | ... | |
| 114 | 117 |
bfs_d2=(temp == -1) ? bfs_d2 : temp; |
| 115 | 118 |
usb_puti(bfs_d2); |
| 116 | 119 |
|
| 120 |
if (bfs_d2 < BFS_SLOW_DISTANCE) |
|
| 121 |
bfs_speed = BFS_SLOW_SPEED; |
|
| 122 |
|
|
| 117 | 123 |
if (bfs_d2 < BFS_ORBIT_DISTANCE) {
|
| 118 |
bfs_state = BFS_ORBIT; // move to orbit state |
|
| 119 |
orbit_init(bfs_follow_id); |
|
| 120 |
bfs_d2=1000; |
|
| 124 |
if (bfs_follow_id != bfs_otherRobot) {
|
|
| 125 |
// check if we can now see a robot that's closer to our goal |
|
| 126 |
temp = bfs_follow(); |
|
| 127 |
wl_do(); |
|
| 128 |
if (temp == BFS_NO_VAL || temp == bfs_follow_id) |
|
| 129 |
bfs_state = BFS_SEEK; |
|
| 130 |
else if (temp != bfs_follow_id) |
|
| 131 |
bfs_follow_id = temp; |
|
| 132 |
return; // redo this function in light of the state changes |
|
| 133 |
} else {
|
|
| 134 |
// orbit the robot |
|
| 135 |
bfs_state = BFS_ORBIT; // move to orbit state |
|
| 136 |
orbit_init(bfs_follow_id); |
|
| 137 |
bfs_d2=1000; |
|
| 138 |
} |
|
| 121 | 139 |
} |
| 122 | 140 |
} |
| 123 | 141 |
} |
| ... | ... | |
| 129 | 147 |
bfs_state = BFS_STOP; // move to stop state |
| 130 | 148 |
} |
| 131 | 149 |
if (bfs_state == BFS_STOP && bfs_follow_id != bfs_otherRobot) {
|
| 132 |
bfs_state = BFS_SEEK; |
|
| 150 |
bfs_state = BFS_SEEK; // seek the otherRobot
|
|
| 133 | 151 |
return; |
| 134 | 152 |
} |
| 135 | 153 |
|
| ... | ... | |
| 148 | 166 |
|
| 149 | 167 |
case(BFS_FOLLOW): orb_set_color(GREEN); |
| 150 | 168 |
// follow another robot |
| 151 |
move(BFS_STRAIGHT_SPEED,bfs_pControl);
|
|
| 169 |
move(bfs_speed,bfs_pControl);
|
|
| 152 | 170 |
break; |
| 153 | 171 |
|
| 154 | 172 |
case(BFS_ORBIT): //orb_set_color(BLUE); |
Also available in: Unified diff