Project

General

Profile

Revision 258

cleaned up lemmings code
added more comments

View differences:

branches/lemmings/code/behaviors/lemmings/driver.c
7 7
#include <wl_token_ring.h>
8 8
#include "lemmings.h"
9 9

  
10
/* ORB COLORS
11
  RED: light blue
12
  ORANGE: light blue
13
  YELLOW: light blue
14
  GREEN:
15
  BLUE:
16
*/
17 10

  
18 11
int main(void) {
12
  // enable everything
19 13
  dragonfly_init(ALL_ON);
20 14
  orb_enable();
21

  
22 15
  wl_init();
23 16
  wl_token_ring_register();
24 17
  wl_token_ring_join(); // join token ring
......
27 20

  
28 21
  while(1) {
29 22
    wl_do();
30
    lemmings_FSM();
23
    lemmings_FSM(); // do lemmings
31 24
  }
32 25

  
33 26
  return 0;
branches/lemmings/code/behaviors/lemmings/lemmings.c
3 3
#include <wireless.h>
4 4
#include <wl_token_ring.h>
5 5
//#include <colonet_dragonfly.h>
6
#include "smart_run_around_fsm.h"
6
//#include "smart_run_around_fsm.h"
7 7
#include "lemmings.h"
8 8

  
9
/*A simple behavior for following the leader.
9
/*A simple behavior for following the leader
10 10
  SINGLE_FILE pattern not implemented yet
11 11
*/
12 12

  
13 13
// FSM states
14
#define LEAD 0 // move around randomly, leading a chain
15
#define FOLLOW 1	// follow another bot
14
#define LEAD 0    // be a leader
15
#define FOLLOW 1	// follow another robot
16 16

  
17 17
#define FOLLOW_MULTI_DEFAULT 1 // set default multi-following pattern
18
#define SPEED 200
18
#define SPEED 200 // set default speed
19 19

  
20 20
/* Globals */
21 21
static int cur_state; 		// current state
22
static int local_leader; // follow this bot (-1 if leader)
23
static int global_leader;
24
static int follow_multi;		// set to 0 for single-file following, 1 for tree following
25
static int local_id;
26
static int num_robots;
22
static int local_leader;  // follow this bot (-1 if leader)
23
static int global_leader; // this is the global leader
24
static int follow_multi;	// set to 0 for single-file following, 1 for tree following
25
static int local_id;      // this is the local robot id
27 26

  
28 27
/* Internal prototypes */
29 28
static int get_local_leader(void);
......
33 32
void lemmings_init() {
34 33
  // usb_puts("lemmings_init\n");
35 34

  
36
//  colonet_init();
37
    //run_around_init(); // prepare for moving
35
  //  colonet_init();
36
  //run_around_init(); // prepare for moving
38 37

  
38
  // set to defaults
39 39
  cur_state = FOLLOW;
40 40
  local_leader = -1;
41 41
  global_leader = -1;
42 42
  local_id = wl_get_xbee_id();
43
  num_robots = 0;
44 43

  
45 44
  follow_multi = FOLLOW_MULTI_DEFAULT;
46 45
}
......
52 51

  
53 52

  
54 53
  if (wl_token_get_num_robots() <= 1) {
54
    /* if there is only one robot in the token ring, there is nothing to
55
       follow, so don't do anything */
55 56
    motors_off();
56 57
    return;
57 58
  }
......
60 61
  //usb_puti(cur_state);
61 62
  //usb_puts("\n");
62 63

  
63
  // Elect a leader if number of robots changes -- max id robot.
64
/*   usb_puts("num robots: "); */
65
/*   usb_puti(num_robots); */
66
/*   usb_puts("\nnew num robots: "); */
67
/*   usb_puti(new_num_robots); */
68
/*   usb_puts("\n"); */
69

  
64
  // only check for new global leader once every 5 iterations
70 65
  leader_check_count++;
71 66
  if (leader_check_count % 5 == 0) {
72 67
    wl_token_iterator_begin();
73 68

  
69
    // find robot with max id and assign as leader
74 70
    int max = -1;
75 71
    while (wl_token_iterator_has_next()) {
76 72
      int n = wl_token_iterator_next();
......
82 78
    }
83 79
    
84 80
/*     usb_puti(local_id); */
85
/*     usb_puts(" is the local \n"); */
81
/*     usb_puts(" is the local robot\n"); */
86 82

  
87 83
    if (max == local_id) {
84
      // become the global leader
88 85
      orb_set_color(BLUE);
89 86
      cur_state = LEAD;
87
      local_leader = -1;
90 88
//      usb_puts(" becoming leader \n");
91 89
    } else {
90
      // become a follower
92 91
      orb_set_color(RED);
93 92
      cur_state = FOLLOW;
94 93
//      usb_puts(" becoming follower\n");
95 94
    }
96 95

  
97
    global_leader = max;
96
    global_leader = max; // set the global leader
98 97
  }
99 98

  
100 99
  switch (cur_state) {
101 100
  case LEAD:
101
    // do lead action - move straight ahead
102 102
    orb_set_color(BLUE);
103

  
104
    //if (local_leader != -1) {
105
      //usb_puts("warning: in LEAD state, but local_leader != -1.\n");
106
    // }
107

  
108 103
    move(SPEED, 0);
109
//    motors_off();
110
// run_around_FSM();
111 104
    break;
112 105

  
113 106
  case FOLLOW:
114 107
    orb_set_color(RED);
115 108

  
116
    // Make sure we can see our local leader.
117
    leader_bom = wl_token_get_sensor_reading(local_id, local_leader);
109
    // Make sure we can see our local leader
110
    leader_bom = (local_leader>=0)?wl_token_get_sensor_reading(local_id, local_leader):-1;
118 111

  
119 112
    if (leader_bom < 0) {
120 113
      //usb_puts("no connection to leader\n");
......
123 116
      local_leader = get_local_leader();
124 117
      
125 118
      if (local_leader == -1) {
119
        // nothing to follow, so become a local_leader and start leading
126 120
        //usb_puts("can't see anyone - becoming a leader\n");
127 121
        cur_state = LEAD;
122
        local_leader = -1;
123
        orb_set_color(BLUE);
124
        move(SPEED, 0);
125
        return;
128 126
      } else {
129 127
        //usb_puts("new leader is");
130 128
        //usb_puti(local_leader);
131 129
        //usb_puts("\n");
132

  
130
        // get data in order to follow leader
133 131
        leader_bom = wl_token_get_sensor_reading(local_id, local_leader);
134 132
        get_bom_velocity(leader_bom, &turn_speed, &speed);
135 133
      }
136 134
    } else {
137
      // Leader is still in sight.
135
      // Leader is still in sight, so follow it
138 136
      get_bom_velocity(leader_bom, &turn_speed, &speed);
139 137
    }
140

  
141
    move(speed, turn_speed); // move in direction of bot
142
    break;
143

  
144
    // default:
145
    // usb_puts("ERROR - invalid state.");
146
  }
147

  
138
    
148 139
  //usb_puts("following robot# ");
149 140
  //usb_puti(local_leader);
150 141
  //usb_puts(" bom of leader: ");
151 142
  //usb_puti(leader_bom);
152 143
  //usb_puts("\n");
144

  
145
    move(speed, turn_speed); // move in direction of leader
146
    break;
147
  }
153 148
}
154 149

  
150
// translate bom reading into direction and speed
155 151
static void get_bom_velocity(int bom_id, int* turn_speed, int* speed) {
156 152
  int error = 0;
157 153

  
......
162 158
    return;
163 159
  }
164 160

  
161
  // set so right is negative, left is positive
165 162
  if (bom_id <= 12) {
166 163
    error = bom_id - 4;
167 164
  } else {
168 165
    error = bom_id - 20;
169 166
  }
170 167

  
171
  *turn_speed = error * 20;
172
  *speed = -SPEED * error * error / 25 + SPEED;
168
  *turn_speed = error * 20; // set turn speed
169
  *speed = -SPEED * error * error / 25 + SPEED; // set speed
173 170
}
174 171

  
175 172
// create connected components and pick a leader for each chain
176
// use modified Prim's alogrithm to find local spanning tree
177 173
// CURRENTLY JUST LOOKS FOR CLOSEST ROBOT TO FOLLOW
178 174
static int get_local_leader(void) {
179 175
  int nodeB;
......
183 179

  
184 180
  wl_token_iterator_begin();
185 181

  
182
  // iterator through robots in token ring
186 183
  while (wl_token_iterator_has_next()) {
187 184
    nodeB = wl_token_iterator_next();
188 185
    if (nodeB == local_id) {
189 186
      continue; // can't follow self
190 187
    }
191 188

  
189
    // get an edge weight based on the robot position
192 190
    cur_weight = get_edge_weight(local_id, nodeB);
193 191

  
194 192
    if (nodeB == global_leader && cur_weight != -1) {
193
      // always follow the global leader if you have the option to
195 194
      best_node = nodeB;
196 195
      break;
197 196
    }

Also available in: Unified diff