Project

General

Profile

Statistics
| Revision:

root / branches / orbit / code / behaviors / bfs_fsm / bfs_fsm.c @ 630

History | View | Annotate | Download (7.46 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("|");
63
  
64
    
65
  if (bfs_state == BFS_SEEK) {
66
    bfs_follow_id = bfs_follow();
67
    usb_puti(bfs_follow_id);
68
    if (bfs_follow_id != BFS_NO_VAL && bfs_follow_id < BFS_MAX_ROBOTS) {
69
      bfs_state = BFS_FOLLOW; // move to follow state
70
      bfs_d2=1000;
71
    }
72
  }
73
  if (bfs_state == BFS_FOLLOW) {
74
    // get bom reading
75
    temp = wl_token_get_my_sensor_reading(bfs_follow_id);
76
    if (temp == -1 || temp > 16) {
77
      bfs_bom_count++;
78
      if (bfs_bom_count > BFS_MAX_BOM_COUNT) {
79
        bfs_state = BFS_SEEK;
80
        bfs_bom = 255;
81
        bfs_bom_count = 0;
82
      }
83
    }
84
    else {
85
      bfs_bom = temp;
86
          
87
      // modify bom reading so right is negative, left is positive
88
      if (bfs_bom <= 12)
89
        bfs_bom -= 4;
90
      else
91
        bfs_bom -= 20;
92
      
93
      bfs_pControl = bfs_bom*4;
94
      usb_puti(bfs_bom);
95
      
96
      
97
      // get range reading for front
98
      temp=range_read_distance(IR2);
99
      bfs_d2=(temp == -1) ? bfs_d2 : temp;
100
      usb_puti(bfs_d2);
101
      
102
      if (bfs_d2 < BFS_ORBIT_DISTANCE) {
103
        bfs_state = BFS_ORBIT; // move to orbit state
104
        orbit_init(bfs_follow_id);
105
        bfs_d2=1000;
106
      }
107
    }  
108
  }
109
  if (bfs_state == BFS_ORBIT) {   
110
    usb_puts("orbiting\n");
111
    while(orbit_state != ORBIT_STOP)
112
    { orbit_fsm(); }
113
    bfs_state = BFS_STOP; // move to stop state  
114
  }
115
  
116
  // evaluate state
117
  bfs_evaluate_state();
118
}
119

    
120

    
121
//Acts on state change.
122
void bfs_evaluate_state(){
123
    switch(bfs_state){
124
    case(BFS_SEEK): orb_set_color(RED);
125
      // move around
126
      run_around_FSM(); // note: better to just incorporate into this file later one
127
      break;
128
    
129
    case(BFS_FOLLOW): orb_set_color(GREEN);
130
      // follow another robot
131
      move(BFS_STRAIGHT_SPEED,bfs_pControl);
132
      break;
133
      
134
    case(BFS_ORBIT): //orb_set_color(BLUE);
135
      // orbit a robot
136
      //orbit_fsm();
137
      //move(0,0);
138
      break;
139
      
140
    case(BFS_STOP): orb_set_color(YELLOW);
141
      // stop
142
      move(0,0);
143
      break;
144
      
145
    default: orb_set_color(MAGENTA);
146
      /*Should never get here, so stop.*/
147
      move(0,0);
148
      break;
149
  }
150
}
151

    
152
/* find a robot to follow using BFS 
153
  ported from colonybfs by Felix
154
*/
155
int bfs_follow()
156
{
157
  
158
/* pseudocode for BFS
159

160
procedure bfs(v)
161
    q := make_queue()
162
    enqueue(q,v)
163
    mark v as visited
164
    while q is not empty
165
        v = dequeue(q)
166
        process v
167
        for all unvisited vertices v' adjacent to v
168
            mark v' as visited
169
            enqueue(q,v')
170
*/
171

    
172
  Queue* q = queue_create();
173

    
174
  //int num_current_robots = wl_token_get_robots_in_ring();
175

    
176
  // keep track of which nodes you have visited.  Indexed by robot #
177
  // 1 if visited, 0 if not
178
  unsigned char visited_nodes[BFS_MAX_ROBOTS];
179
  // keep track of the distance from the start robot to other robots
180
  // also indexed by robot#
181
  unsigned char node_distances[BFS_MAX_ROBOTS];
182
  // this is the path you take
183
  unsigned char path[BFS_MAX_ROBOTS];
184
    
185
  //variables you will need
186
  unsigned char current_node;                 //current node
187
  unsigned char current_neighbour_node;       //neighbor to the current node
188
  unsigned char current_neighbour_val;        //value of that neighbour's sensors
189
  unsigned char current_distance;              //keep track of current distance to the start
190
    
191
  unsigned char large_number = 255;
192
  
193
  unsigned char* add_value;
194
  
195
  
196

    
197
  //set visited nodes to all 0
198
  memset(visited_nodes, 0, BFS_MAX_ROBOTS);
199
  
200
  //set all the distances to a very large number
201
  //this isn't technically necessary, but it's probably a good thing -- just in case
202
  memset(node_distances, large_number, BFS_MAX_ROBOTS);
203
  
204
  //set the path to all LARGE_NUMBER as well
205
  memset(path, BFS_NO_VAL, BFS_MAX_ROBOTS);
206

    
207
  //queue_remove_all(q);
208

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

    
283
}
284

    
285