Project

General

Profile

Statistics
| Revision:

root / trunk / code / behaviors / bfs_fsm / bfs_fsm.c @ 678

History | View | Annotate | Download (7.99 KB)

1
#include <dragonfly_lib.h>
2
#include <wireless.h>
3
#include <wl_token_ring.h>
4
#include "queue.h"
5
#include <string.h>
6
#include "bfs_fsm.h"
7
#include "smart_run_around_fsm.h"
8
#include "orbit_fsm.h"
9

    
10
/* Used to find a robot (or other object)
11

12
  Uses bom and token ring
13
  
14
  Assumes:
15
    robots are already in a token ring
16
*/
17

    
18

    
19
/* private function prototypes */
20
void bfs_evaluate_state(void);
21
int bfs_follow(void);
22

    
23

    
24
/* init */
25
void bfs_init(int robot) {
26
  range_init();
27
  analog_init(1);
28
  motors_init();
29
  orb_init();
30
  orb_enable();
31
  usb_init();
32
  run_around_init();
33
 
34
  /*Start in the start state, BFS_SEEK */ 
35
  bfs_state = BFS_SEEK;
36
  
37
  bfs_otherRobot = robot; // set which robot to seek
38
  bfs_my_id = wl_get_xbee_id();
39
  bfs_follow_id = -1;
40
  
41
  bfs_pControl=0;
42
  bfs_bom = 0;
43
  bfs_bom_count = 0;
44

    
45
  
46
  /*Initialize distances to zero.*/ 
47
  bfs_d1=1000; bfs_d2=1000; bfs_d3=1000; bfs_d4=1000; bfs_d5=1000;
48
  
49
}
50

    
51
/*The main function, call this to update states as frequently as possible.*/
52
void bfs_fsm(void) {
53
  
54
  /*The following lines ensure that undefined (-1) values
55
  will not update the distances.*/ 
56
  int temp;  
57
    
58
  wl_do(); // update wireless
59
  usb_puts("\n\rwl_do");
60
  usb_puts("|state=");
61
  usb_puti(bfs_state);
62
  usb_puts("|bfs_follow=");
63
  usb_puti(bfs_follow_id);
64
  usb_puts("|");
65
  
66
    
67
  if (bfs_state == BFS_SEEK) {
68
    bfs_follow_id = bfs_follow();
69
    wl_do(); // update wireless
70
    usb_puti(bfs_follow_id);
71
    if (bfs_follow_id != BFS_NO_VAL && bfs_follow_id < BFS_MAX_ROBOTS) {
72
      bfs_state = BFS_FOLLOW; // move to follow state
73
      bfs_d2=1000;
74
      bfs_check_id=0;
75
    }
76
  }
77
  if (bfs_state == BFS_FOLLOW) {
78
    /* check robot to follow */
79
    if (++bfs_check_id > BFS_CHECK_ID_TIME) {
80
      bfs_check_id=0;
81
      temp = bfs_follow();
82
      wl_do();
83
      if (temp == BFS_NO_VAL) {
84
        bfs_state = BFS_SEEK;
85
        return;
86
      } else if (temp != bfs_follow_id)
87
        bfs_follow_id = temp;
88
    }
89
    // get bom reading
90
    temp = wl_token_get_my_sensor_reading(bfs_follow_id);
91
    if (temp == -1 || temp > 16) {
92
      bfs_bom_count++;
93
      if (bfs_bom_count > BFS_MAX_BOM_COUNT) {
94
        bfs_state = BFS_SEEK;
95
        bfs_bom = 255;
96
        bfs_bom_count = 0;
97
      }
98
    }
99
    else {
100
      bfs_bom = temp;
101
          
102
      // modify bom reading so right is negative, left is positive
103
      if (bfs_bom <= 12)
104
        bfs_bom -= 4;
105
      else
106
        bfs_bom -= 20;
107
      
108
      bfs_pControl = bfs_bom*4;
109
      usb_puti(bfs_bom);
110
      
111
      
112
      // get range reading for front
113
      temp=range_read_distance(IR2);
114
      bfs_d2=(temp == -1) ? bfs_d2 : temp;
115
      usb_puti(bfs_d2);
116
      
117
      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;
121
      }
122
    }  
123
  }
124
  if (bfs_state == BFS_ORBIT) {   
125
    usb_puts("orbiting\n");
126
    orbit_state = ORBIT_INSERTION;
127
    while(orbit_state != ORBIT_STOP)
128
    { orbit_fsm(); }
129
    bfs_state = BFS_STOP; // move to stop state  
130
  }
131
  if (bfs_state == BFS_STOP && bfs_follow_id != bfs_otherRobot) {
132
    bfs_state = BFS_SEEK;
133
    return;
134
  }
135
  
136
  // evaluate state
137
  bfs_evaluate_state();
138
}
139

    
140

    
141
//Acts on state change.
142
void bfs_evaluate_state(){
143
    switch(bfs_state){
144
    case(BFS_SEEK): orb_set_color(RED);
145
      // move around
146
      run_around_FSM(); // note: better to just incorporate into this file later one
147
      break;
148
    
149
    case(BFS_FOLLOW): orb_set_color(GREEN);
150
      // follow another robot
151
      move(BFS_STRAIGHT_SPEED,bfs_pControl);
152
      break;
153
      
154
    case(BFS_ORBIT): //orb_set_color(BLUE);
155
      // orbit a robot
156
      //orbit_fsm();
157
      //move(0,0);
158
      break;
159
      
160
    case(BFS_STOP): orb_set_color(YELLOW);
161
      // stop
162
      move(0,0);
163
      break;
164
      
165
    default: orb_set_color(MAGENTA);
166
      /*Should never get here, so stop.*/
167
      move(0,0);
168
      break;
169
  }
170
}
171

    
172
/* find a robot to follow using BFS 
173
  ported from colonybfs by Felix
174
*/
175
int bfs_follow()
176
{
177
  
178
/* pseudocode for BFS
179

180
procedure bfs(v)
181
    q := make_queue()
182
    enqueue(q,v)
183
    mark v as visited
184
    while q is not empty
185
        v = dequeue(q)
186
        process v
187
        for all unvisited vertices v' adjacent to v
188
            mark v' as visited
189
            enqueue(q,v')
190
*/
191

    
192
  Queue* q = queue_create();
193

    
194
  //int num_current_robots = wl_token_get_robots_in_ring();
195

    
196
  // keep track of which nodes you have visited.  Indexed by robot #
197
  // 1 if visited, 0 if not
198
  unsigned char visited_nodes[BFS_MAX_ROBOTS];
199
  // keep track of the distance from the start robot to other robots
200
  // also indexed by robot#
201
  unsigned char node_distances[BFS_MAX_ROBOTS];
202
  // this is the path you take
203
  unsigned char path[BFS_MAX_ROBOTS];
204
    
205
  //variables you will need
206
  unsigned char current_node;                 //current node
207
  unsigned char current_neighbour_node;       //neighbor to the current node
208
  unsigned char current_neighbour_val;        //value of that neighbour's sensors
209
  unsigned char current_distance;              //keep track of current distance to the start
210
    
211
  unsigned char large_number = 255;
212
  
213
  unsigned char* add_value;
214
  
215
  
216

    
217
  //set visited nodes to all 0
218
  memset(visited_nodes, 0, BFS_MAX_ROBOTS);
219
  
220
  //set all the distances to a very large number
221
  //this isn't technically necessary, but it's probably a good thing -- just in case
222
  memset(node_distances, large_number, BFS_MAX_ROBOTS);
223
  
224
  //set the path to all LARGE_NUMBER as well
225
  memset(path, BFS_NO_VAL, BFS_MAX_ROBOTS);
226

    
227
  //queue_remove_all(q);
228

    
229
  //add the start node
230
  add_value = (unsigned char*)malloc(sizeof(char));
231
  (*add_value) = bfs_my_id;
232
  queue_add(q, add_value);
233
  visited_nodes[bfs_my_id] = 1;
234
  node_distances[bfs_my_id] = 0;
235
  
236
  while(!queue_is_empty(q)){
237
    add_value = queue_remove(q);
238
    current_node = (*add_value);
239
    
240
    //get the current distance from you to the start
241
    current_distance = node_distances[current_node];
242
    
243
    //this node is on your 'path'
244
    path[current_distance] = current_node;
245
    //note: it's OK if this path leads nowhere -- the path will be
246
    //overwritten by a node that does lead to the goal
247
    //(if there is no path, we set path to null anyways)
248
    
249
    //from now on, all further nodes will be one further away -- increment
250
    current_distance++;
251
    
252
    
253
    //process node -- basically check if it's the goal
254
    if(current_node == bfs_otherRobot) {
255
      //you reach the goal
256
      //return the first robot in the path, which is the one
257
      //to follow
258
      /*
259
      lcd_putchar('.');
260
      lcd_putchar('.');
261
      for(i = 0; i < MAX_ROBOTS; i++)
262
          lcd_putchar(path[i] +48);           //print out the path for fun -- remove this later
263
      lcd_putchar('.');
264
      lcd_putchar('.');
265
      */
266
      return path[1];         //path[0] is you.  path[1] is who you want to follow
267
    }
268
    
269
    // go through all nodes adjacent to current
270
    // in effect, this means going through the current node's row in the matrix
271
    
272
    wl_token_iterator_begin();
273
    while(wl_token_iterator_has_next()) {
274
      //the robot # is actually just the index
275
      current_neighbour_node = wl_token_iterator_next();
276
      //the value is what is stored in the matrix (this is important)
277
      current_neighbour_val = wl_token_get_sensor_reading(current_node,current_neighbour_node);
278
      
279
      //check for connected-ness and that it was not visited
280
      //            unconnected                           visited
281
      if( (current_neighbour_val == BFS_NO_VAL) || (visited_nodes[current_neighbour_node]) ) {
282
        //if it is either unconnected or previously visited, exit
283
        continue;   //go to the next node
284
      }
285
      
286
      //update the distance from the start node to this node
287
      node_distances[current_neighbour_node] = current_distance;
288
      //also mark it as visited
289
      visited_nodes[current_neighbour_node] = 1;
290
      
291
      //also enqueue each neighbour
292
      add_value = (unsigned char*)malloc(sizeof(char));
293
      (*add_value) = current_neighbour_node;
294
      queue_add(q, add_value);
295
    
296
    }
297
  }
298
  
299
  //if we get to here, there is no path
300
  memset(path, 0, BFS_MAX_ROBOTS);
301
  return BFS_NO_VAL;
302

    
303
}
304

    
305