Project

General

Profile

Statistics
| Revision:

root / branches / lemmings / code / behaviors / lemmings_dsschult / lemmings.c @ 209

History | View | Annotate | Download (5.18 KB)

1
#include <dragonfly_lib.h>
2
#include <wl_defs.h>
3
#include <wireless.h>
4
#include <wl_token_ring.h>
5
//#include "smart_run_around_fsm.h"
6
#include "lemmings.h"
7

    
8
/*A simple behavior for following the leader.
9

10
  SINGLE_FILE pattern not implemented yet
11
*/
12

    
13

    
14
void lemmings_init()
15
{
16
  //run_around_init(); // prepare for moving
17
  dragonfly_init(ALL_ON);
18
  analog_init();
19
  motors_init();
20
  range_init();
21
  orb_init();
22
  orb_enable();
23
  usb_init();
24
  wl_init();
25
  wl_token_ring_register();
26
  wl_token_ring_join(); // join token ring
27
 
28
  /*Start in the default state, LEAD*/ 
29
  cur_state = LEAD;
30
  /*Set timers to their maximum values.*/
31
  crazy_count=CRAZY_MAX;
32
  
33
  // set to default state
34
  follow_bot = -1;
35
  cur_state = LEAD;
36
  
37
  usb_puts("init_following: ");
38
  usb_puti(follow_bot);
39
  
40
  // set pattern
41
  lemmings_set_pattern(FOLLOW_MULTI_DEFAULT);
42
}
43

    
44
// set the following pattern
45
// SINGLE_FILE if pattern <= 0
46
// TREE        if pattern >= 1
47
void lemmings_set_pattern(int pattern)
48
{
49
  if (pattern <= 0)
50
    follow_multi = 0;
51
  else
52
    follow_multi = 1;
53
  orb_set_color(ORANGE);
54
}
55

    
56
/*The main function, call this to update states as frequently as possible.*/
57
void lemmings_FSM(void) {
58
  int bom=0;
59
  int i=0;
60
  int old_follow_bot;
61
  
62
  orb_set_color(WHITE);
63
  if (follow_bot < 0)
64
    lemmings_Prims(); // if we are not following, then attempt to follow
65
    
66
  usb_puts("following: ");
67
  usb_puti(follow_bot);
68
  orb_set_color(GREEN);
69
  // if we are following a bot, make sure we can still see it
70
  old_follow_bot = follow_bot;
71
  for (i=0;follow_bot>=0&&i<10;i++) {
72
    bom = wl_token_get_sensor_reading(wl_get_xbee_id(),follow_bot);
73
    if (bom < 0) {
74
      // get new bot to follow
75
      lemmings_Prims();
76
          if (old_follow_bot == follow_bot)
77
                break; // some sort of error
78
      continue;
79
    }
80
    else {
81
      cur_state = FOLLOW; // set to follow state
82
      switch(bom) {
83
      case 0:
84
      case 15:
85
      case 14:
86
      case 13:
87
      case 12:
88
        follow_direction = 255;
89
        break;
90
      case 1:
91
        follow_direction = 150;
92
        break;
93
      case 2:
94
        follow_direction = 100;
95
        break;
96
      case 3:
97
        follow_direction = 50;
98
        break;
99
      case 4:
100
        follow_direction = 0;
101
        break;
102
      case 5:
103
        follow_direction = -50;
104
        break;
105
      case 6:
106
        follow_direction = -100;
107
        break;
108
      case 7:
109
        follow_direction = -150;
110
        break;
111
      case 8:
112
      case 9:
113
      case 10:
114
      case 11:
115
        follow_direction = -255;
116
        break;
117
      default:
118
        follow_direction = 0;
119
        break;
120
      }
121
      break; // break from loop
122
    }
123
  }
124

    
125
  
126
  /*If the crazy count is in it's >>3 range, and we are currently a follower, be a leader.*/
127
  /* CURRENTLY DISABLED
128
  if(cur_state == FOLLOW && crazy_count<=(CRAZY_MAX>>3))
129
  {
130
    cur_state = LEAD;
131
  }*/
132
  
133
  lemmings_evaluate_state(); // evaluate state
134
}
135

    
136
// create connected components and pick a leader for each chain
137
// use modified Prim's alogrithm to find local spanning tree
138
// CURRENTLY JUST LOOKS FOR CLOSEST ROBOT TO FOLLOW
139
void lemmings_Prims(void) {
140
  
141
  int nodeA = wl_get_xbee_id(); // get id of current robot
142
  int nodeB = -1;
143
  int cur_weight = -1;
144
  int best_weight = 1000; // set to infinity initially
145
  int best_node = -1;
146
  int i=0;
147
  orb_set_color(BLUE);
148
  
149
  // iterate through token ring
150
  wl_token_iterator_begin();
151
  for(i=0;wl_token_iterator_has_next()&&i<10;i++) {
152
    nodeB = wl_token_iterator_next();
153
    usb_puts("botB: ");
154
    usb_puti(nodeB);
155
    usb_puts("\n\r");
156
    if (nodeB < 0)
157
      break; // no more bots
158
    else if (nodeB == nodeA)
159
      continue; // can't follow self
160
    
161
    cur_weight = lemmings_get_edge_weight(nodeA,nodeB);
162
    if (cur_weight < best_weight) {
163
      // this is new best node, so save values
164
      best_weight = cur_weight;
165
      best_node = nodeB;
166
    }
167
  }
168
  
169
  if (best_node == -1 || (best_weight > 5 && nodeA < nodeB)) {
170
    // no robots to follow, or choosing to be a leader
171
    cur_state = LEAD;
172
    follow_bot = -1;
173
  }
174
  else {
175
    // follow bot
176
    cur_state = FOLLOW;
177
    follow_bot = nodeB;
178
  }
179
}
180

    
181
// get edge weight of sensor matrix
182
// add in BOM range data when BOM 1.5 comes out
183
int lemmings_get_edge_weight(int robot1, int robot2) {
184
  int weight = -1;
185
  int bom = wl_token_get_sensor_reading(robot1,robot2);
186
  // since we only have positioning data, give better weight to
187
  // bots in from of us
188
  switch(bom) {
189
  case 12:
190
    weight++;
191
  case 13:
192
  case 11:
193
    weight++;
194
  case 14:
195
  case 10:
196
    weight++;
197
  case 15:
198
  case 9:
199
    weight++;
200
  case 0:
201
  case 8:
202
    weight++;
203
  case 1:
204
  case 7:
205
    weight++;
206
  case 2:
207
  case 6:
208
    weight++;
209
  case 3:
210
  case 5:
211
    weight++;
212
  case 4:
213
    break;
214
  default:
215
    weight = -1;
216
    break;
217
  }
218
  return weight;
219
}
220

    
221

    
222
//Acts on state change.
223
void lemmings_evaluate_state(){
224
  usb_puts("state: ");
225
  usb_puti(cur_state);
226
  usb_puts("\n\r");
227
  switch(cur_state){
228
    case(LEAD): 
229
      move_avoid(200,0,50);
230
      orb_set_color(GREEN);
231
      //run_around_FSM(); // do random run around
232
      break;
233
    
234
    case(FOLLOW): 
235
      move(200,follow_direction); // move in direction of bot
236
      orb_set_color(RED);
237
      break;
238
      
239
    default:
240
      /*Should never get here, go strait.*/
241
          move(200,0);
242
          orb_set_color(BLUE);
243
          break;
244
        }
245
}