Project

General

Profile

Revision 1345

Added by Rich Hong almost 11 years ago

Final spline code for master/slave

updated outdated libdragonfly and libwireless

View differences:

wl_token_ring.c
1
/**
2
 * Copyright (c) 2007 Colony Project
3
 *
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 *
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 *
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

  
26
/**
27
 * @file wl_token_ring.c
28
 * @brief Token Ring Implementation
29
 *
30
 * Implementation of the token ring packet group.
31
 *
32
 * @author Brian Coltin, Colony Project, CMU Robotics Club
33
 **/
34

  
1 35
#include <wl_token_ring.h>
2 36

  
3 37
#include <stdlib.h>
......
6 40
#include <wl_defs.h>
7 41
#include <wireless.h>
8 42
#include <sensor_matrix.h>
9
#include <queue.h>
10 43

  
11 44
#ifdef ROBOT
12 45
#ifndef FIREFLY
......
15 48
#include <time.h>
16 49
#endif
17 50

  
51
//TODO: why is this in both this file and sensor_matrix.c?  If it is needed in both places,
52
// put it in sensor_matrix.h.  This file already includes sensor_matrix.h
18 53
#define DEFAULT_SENSOR_MATRIX_SIZE 20
19 54

  
20 55
/*Ring States*/
......
26 61
#define LEAVING 4
27 62

  
28 63
/*Frame Types*/
29
#define TOKEN_JOIN_ACCEPT_FRAME 1
64
#define TOKEN_JOIN_ACCEPT_FRAME	1
65
#define WL_TOKEN_PASS_FRAME	2
30 66

  
31 67
/*Function Prototypes*/
32 68

  
33 69
/*Wireless Library Prototypes*/
34
void wl_token_ring_timeout_handler(void);
35
void wl_token_ring_response_handler(int frame, int received);
36
void wl_token_ring_receive_handler(char type, int source, unsigned char* packet,
37
							int length);
38
void wl_token_ring_cleanup(void);
70
static void wl_token_ring_timeout_handler(void);
71
static void wl_token_ring_response_handler(int frame, int received);
72
static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length);
73
static void wl_token_ring_cleanup(void);
39 74

  
40 75
/*Helper Functions*/
41
void wl_token_pass_token(void);
42
int get_token_distance(int robot1, int robot2);
43
void wl_token_get_token(void);
76
static int wl_token_pass_token(void);
77
static int get_token_distance(int robot1, int robot2);
78
static void wl_token_get_token(void);
44 79

  
45 80
/*Packet Handling Routines*/
46
void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength);
47
void wl_token_interrupt_request_receive(int source, int robot);
48
void wl_token_interrupt_pass_receive(int source, int robot);
49
void wl_token_bom_on_receive(int source);
50
void wl_token_join_receive(int source);
51
void wl_token_join_accept_receive(int source);
81
static void wl_token_pass_receive(int source);
82
static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength);
83
static void wl_token_bom_on_receive(int source);
84
static void wl_token_join_receive(int source);
85
static void wl_token_join_accept_receive(int source);
52 86

  
53 87
/*Global Variables*/
54 88

  
55
//the sensor matrix
56
SensorMatrix* sensorMatrix;
57

  
58 89
//the robot we are waiting to say it has received the token. -1 if unspecified
59
int wl_token_next_robot = -1;
90
static int wl_token_next_robot = -1;
60 91

  
61 92
//true if the robot should be in the token ring, 0 otherwise
62
int ringState = NONMEMBER;
93
static int ringState = NONMEMBER;
63 94
//the id of the robot who accepted us into the token ring, only used in ACCEPTED state
64
int acceptor = -1;
95
static int acceptor = -1;
65 96
//id of the robot we are accepting
66
int accepted = -1;
97
static int accepted = -1;
67 98

  
68 99
//the counter for when we assume a robot is dead
69
int deathDelay = -1;
100
static int deathDelay = -1;
70 101
//the counter for joining, before we form our own token ring
71
int joinDelay = -1;
72
//queue containing ids of interruption requests
73
Queue* interrupting = NULL;
102
static int joinDelay = -1;
74 103

  
75 104
//current robot to check in the iterator
76
int iteratorCount = 0;
105
static int iteratorCount = 0;
77 106

  
78
void do_nothing(void) {}
79
int get_nothing(void) {return -1;}
107
// the amount of time a robot has had its BOM on for
108
static int bom_on_count = 0;
80 109

  
110
#ifndef ROBOT
111
static void do_nothing(void) {}
112
static int get_nothing(void) {return -1;}
113
#endif
114

  
81 115
#ifdef ROBOT
82 116
#ifndef FIREFLY
83
void (*bom_on_function) (void) = bom_on;
84
void (*bom_off_function) (void) = bom_off;
85
int (*get_max_bom_function) (void) = get_max_bom;
117
static void (*bom_on_function) (void) = bom_on;
118
static void (*bom_off_function) (void) = bom_off;
119
static int (*get_max_bom_function) (void) = get_max_bom;
86 120
#else
87
void (*bom_on_function) (void) = do_nothing;
88
void (*bom_off_function) (void) = do_nothing;
89
int (*get_max_bom_function) (void) = get_nothing;
121
static void (*bom_on_function) (void) = do_nothing;
122
static void (*bom_off_function) (void) = do_nothing;
123
static int (*get_max_bom_function) (void) = get_nothing;
90 124
#endif
91 125
#else
92
void (*bom_on_function) (void) = do_nothing;
93
void (*bom_off_function) (void) = do_nothing;
94
int (*get_max_bom_function) (void) = get_nothing;
126
static void (*bom_on_function) (void) = do_nothing;
127
static void (*bom_off_function) (void) = do_nothing;
128
static int (*get_max_bom_function) (void) = get_nothing;
95 129
#endif
96 130

  
97
PacketGroupHandler wl_token_ring_handler =
98
		{WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,
131
static PacketGroupHandler wl_token_ring_handler =
132
	{WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,
99 133
		wl_token_ring_response_handler, wl_token_ring_receive_handler,
100 134
		wl_token_ring_cleanup};
101 135

  
102 136
/**
137
 * Causes the robot to join an existing token ring, or create one
138
 * if no token ring exists. The token ring uses global and robot to robot
139
 * packets, and does not rely on any PAN.
140
 **/
141
int wl_token_ring_join()
142
{
143
	WL_DEBUG_PRINT("Joining the token ring.\r\n");
144

  
145
	ringState = JOINING;
146
	joinDelay = DEATH_DELAY * 2;
147
	if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN, NULL, 0, 0) != 0) {
148
		return -1;
149
	}
150

  
151
	return 0;
152
}
153

  
154
/**
155
 * Causes the robot to leave the token ring. The robot stops
156
 * alerting others of its location, but continues storing the
157
 * locations of other robots.
158
 **/
159
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
160
// it reduces code size or not should be done to be sure.
161
void wl_token_ring_leave()
162
{
163
	ringState = LEAVING;
164
}
165

  
166
/**
103 167
 * Initialize the token ring packet group and register it with the
104 168
 * wireless library. The robot will not join a token ring.
105 169
 **/
106
void wl_token_ring_register()
170
int wl_token_ring_register()
107 171
{
108 172
	if (wl_get_xbee_id() > 0xFF)
109 173
	{
......
112 176
		WL_DEBUG_PRINT("XBee ID must be single byte for token ring, is ");
113 177
		WL_DEBUG_PRINT_INT(wl_get_xbee_id());
114 178
		WL_DEBUG_PRINT(".\r\n");
115
		return;
179
		return -1;
116 180
	}
117
	
118
	sensorMatrix = sensor_matrix_create();
119
	interrupting = queue_create();
181

  
182
	sensor_matrix_create();
120 183
	//add ourselves to the sensor matrix
121
	sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
184
	sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
122 185

  
123 186
	wl_register_packet_group(&wl_token_ring_handler);
187

  
188
	return 0;
124 189
}
125 190

  
126 191
/**
127 192
 * Removes the packet group from the wireless library.
128 193
 **/
194
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
195
// it reduces code size or not should be done to be sure.
129 196
void wl_token_ring_unregister()
130 197
{
131 198
	wl_unregister_packet_group(&wl_token_ring_handler);
......
133 200

  
134 201
/**
135 202
 * Sets the functions that are called when the BOM ought to be
136
 * turned on or off. This could be used for things such as 
203
 * turned on or off. This could be used for things such as
137 204
 * charging stations, which have multiple BOMs.
138 205
 *
139 206
 * @param on_function the function to be called when the BOM
......
154 221
/**
155 222
 * Called to cleanup the token ring packet group.
156 223
 **/
157
void wl_token_ring_cleanup()
224
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
225
// it reduces code size or not should be done to be sure.
226
static void wl_token_ring_cleanup()
158 227
{
159
	sensor_matrix_destroy(sensorMatrix);
160
	queue_destroy(interrupting);
161 228
}
162 229

  
163 230
/**
164 231
 * Called approximately every quarter second by the wireless library.
165 232
 **/
166
void wl_token_ring_timeout_handler()
233
static void wl_token_ring_timeout_handler()
167 234
{
168 235
	//someone is not responding, assume they are dead
169 236
	if (deathDelay == 0)
......
172 239
		//also, declare that person dead, as long as it isn't us
173 240
		if (wl_token_next_robot != wl_get_xbee_id())
174 241
		{
175
			sensor_matrix_set_in_ring(sensorMatrix, wl_token_next_robot, 0);
242
			sensor_matrix_set_in_ring(wl_token_next_robot, 0);
176 243
			WL_DEBUG_PRINT("Robot ");
177 244
			WL_DEBUG_PRINT_INT(wl_token_next_robot);
178 245
			WL_DEBUG_PRINT(" has died.\r\n");
246
			wl_token_next_robot = -1;
247
			deathDelay = DEATH_DELAY;
179 248
		}
180
		
249

  
181 250
		// we may have been dropped from the ring when this is received
182
		if (ringState == MEMBER)
251
		if (ringState == MEMBER) {
183 252
			wl_token_pass_token();
253
		}
184 254
	}
185 255

  
186 256
	//we must start our own token ring, no one is responding to us
187 257
	if (joinDelay == 0)
188 258
	{
189
		if (sensor_matrix_get_joined(sensorMatrix) == 0)
259
		if (sensor_matrix_get_joined() == 0)
190 260
		{
191 261
			WL_DEBUG_PRINT("Creating our own token ring, no robots seem to exist.\r\n");
192
			sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
262
			sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
193 263
			ringState = MEMBER;
194 264
			//this will make us pass the token to ourself
195 265
			//repeatedly, and other robots when they join
......
205 275
		}
206 276
	}
207 277

  
208
	if (deathDelay >= 0)
278
	if (deathDelay >= 0) {
209 279
		deathDelay--;
210
	if (joinDelay >= 0)
280
	}
281

  
282
	if (joinDelay >= 0) {
211 283
		joinDelay--;
284
	}
285

  
286
	if (bom_on_count >= 0) {
287
		bom_on_count++;
288
	}
212 289
}
213 290

  
214 291
/**
215 292
 * Called when the XBee tells us if a packet we sent has been received.
216
 * 
293
 *
217 294
 * @param frame the frame number assigned when the packet was sent
218 295
 * @param received 1 if the packet was received, 0 otherwise
219 296
 **/
220
void wl_token_ring_response_handler(int frame, int received)
297
static void wl_token_ring_response_handler(int frame, int received)
221 298
{
222 299
	if (!received)
223 300
	{
......
232 309
 * @param packet the data in the packet
233 310
 * @param length the length of the packet in bytes
234 311
 **/
235
void wl_token_ring_receive_handler(char type, int source, unsigned char* packet,
236
							int length)
312
static void wl_token_ring_receive_handler(char type, int source, unsigned char* packet, int length)
237 313
{
238 314
	switch (type)
239 315
	{
240 316
		case WL_TOKEN_PASS:
241
			if (length < 1)
242
			{
243
				WL_DEBUG_PRINT("Malformed Token Pass packet received.\r\n");
244
				return;
245
			}
246
			wl_token_pass_receive(source, packet[0], packet + 1, length - 1);
317
			wl_token_pass_receive(source);
247 318
			break;
319
		case WL_TOKEN_SENSOR_MATRIX:
320
			wl_token_sensor_matrix_receive(source, packet, length);
321
			break;
248 322
		case WL_TOKEN_BOM_ON:
249 323
			//add the robot to the sensor matrix if it is not already there
250 324
			wl_token_bom_on_receive(source);
251 325
			break;
252
		case WL_TOKEN_INTERRUPT_REQUEST:
253
			wl_token_interrupt_request_receive(source, packet[0]);
254
			break;
255
		case WL_TOKEN_INTERRUPT_PASS:
256
			wl_token_interrupt_pass_receive(source, packet[0]);
257
			break;
258 326
		case WL_TOKEN_JOIN:
259 327
			wl_token_join_receive(source);
260 328
			break;
......
268 336
}
269 337

  
270 338
/**
271
 * Causes the robot to join an existing token ring, or create one
272
 * if no token ring exists. The token ring uses global and robot to robot
273
 * packets, and does not rely on any PAN.
339
 * Returns the BOM reading robot source has for robot dest.
340
 *
341
 * @param source the robot that made the BOM reading
342
 * @param dest the robot whose relative location is returned
343
 *
344
 * @return a BOM reading from robot source to robot dest,
345
 * in the range 0-15, or -1 if it is unknown
274 346
 **/
275
void wl_token_ring_join()
347
int wl_token_get_sensor_reading(int source, int dest)
276 348
{
277
	WL_DEBUG_PRINT("Joining the token ring.\r\n");
278
	ringState = JOINING;
279
	joinDelay = JOIN_DELAY;
280
	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN,
281
		NULL, 0, 0);
349
	if (wl_token_is_robot_in_ring(dest) &&
350
			(source == wl_get_xbee_id() || wl_token_is_robot_in_ring(source))) {
351
		return sensor_matrix_get_reading(source, dest);
352
	}
353

  
354
	return -1;
282 355
}
283 356

  
284 357
/**
285
 * Causes the robot to leave the token ring. The robot stops
286
 * alerting others of its location, but continues storing the
287
 * locations of other robots.
358
 * Returns the BOM reading we have for robot dest.
359
 *
360
 * @param dest the robot whose relative location is returned
361
 *
362
 * @return a BOM reading from us to robot dest, in the range
363
 * 0-15, or -1 if it is unkown
288 364
 **/
289
void wl_token_ring_leave()
365
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
366
// it reduces code size or not should be done to be sure.
367
int wl_token_get_my_sensor_reading(int dest)
290 368
{
291
	ringState = LEAVING;
369
	return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);
292 370
}
293 371

  
372

  
294 373
/**
295
 * Requests that the specified robot be given the token and
296
 * allowed to flash its BOM. After its BOM is flashed, the
297
 * token will return to the robot who sent it.
374
 * Returns the number of robots in the token ring.
298 375
 *
299
 * @param robot the ID of the robot which should flash its BOM
376
 * @return the number of robots in the token ring
300 377
 **/
301
void wl_token_request(int robot)
378
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
379
// it reduces code size or not should be done to be sure.
380
int wl_token_get_robots_in_ring(void)
302 381
{
303
	char buf[1];
304
	buf[0] = robot;
305
	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_INTERRUPT_REQUEST,
306
		buf, 1, 0);
382
	return sensor_matrix_get_joined();
307 383
}
308 384

  
309 385
/**
310
 * Returns the BOM reading robot source has for robot dest.
386
 * Returns true if the specified robot is in the token ring, false
387
 * otherwise.
311 388
 *
312
 * @param source the robot that made the BOM reading
313
 * @param dest the robot whose relative location is returned
389
 * @param robot the robot to check for whether it is in the token ring
390
 * @return nonzero if the robot is in the token ring, zero otherwise
391
 **/
392
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
393
// it reduces code size or not should be done to be sure.
394
int wl_token_is_robot_in_ring(int robot)
395
{
396
	return sensor_matrix_get_in_ring(robot);
397
}
398

  
399
/**
400
 * Begins iterating through the robots in the token ring.
314 401
 *
315
 * @return a BOM reading from robot source to robot dest,
316
 * in the range 0-15, or -1 if it is unknown
402
 * @see wl_token_iterator_has_next, wl_token_iterator_next
317 403
 **/
318
int wl_token_get_sensor_reading(int source, int dest)
404
void wl_token_iterator_begin(void)
319 405
{
320
	return sensor_matrix_get_reading(sensorMatrix, source, dest);
406
	int i = 0;
407
  //TODO: the compiler may or may not optimize this such that my comment is useless:
408
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
409
  // the overhead of a function call each iteration, call it only once before the loop and store
410
  // the value in a variable and check against that variable in the loop condition
411
	while (!sensor_matrix_get_in_ring(i) && i < sensor_matrix_get_size()) {
412
		i++;
413
	}
414

  
415
  //TODO: if you do the above comment, then you can compare this to the variable also
416
	if (i == sensor_matrix_get_size()) {
417
		i = -1;
418
	}
419

  
420
	iteratorCount = i;
321 421
}
322 422

  
323 423
/**
324
 * Returns the BOM reading we have for robot dest.
325
 * 
326
 * @param dest the robot whose relative location is returned
424
 * Returns true if there are more robots in the token ring
425
 * to iterate through, and false otherwise.
327 426
 *
328
 * @return a BOM reading from us to robot dest, in the range
329
 * 0-15, or -1 if it is unkown
427
 * @return nonzero if there are more robots to iterate through,
428
 * zero otherwise
429
 *
430
 * @see wl_token_iterator_begin, wl_token_iterator_next
330 431
 **/
331
int wl_token_get_my_sensor_reading(int dest)
432
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
433
// it reduces code size or not should be done to be sure.
434
int wl_token_iterator_has_next(void)
332 435
{
333
	return wl_token_get_sensor_reading(wl_get_xbee_id(), dest);
436
	return iteratorCount != -1;
334 437
}
335 438

  
336 439
/**
440
 * Returns the next robot ID in the token ring.
441
 *
442
 * @return the next robot ID in the token ring, or -1 if none exists
443
 *
444
 * @see wl_token_iterator_begin, wl_token_iterator_has_next
445
 **/
446
int wl_token_iterator_next(void)
447
{
448
	int result = iteratorCount;
449
	if (result < 0) {
450
		return result;
451
	}
452

  
453
  //TODO: the compiler may or may not optimize this such that my comment is useless:
454
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
455
  // the overhead of a function call each iteration, call it only once before the loop and store
456
  // the value in a variable and check against that variable in the loop condition
457
	iteratorCount++;
458
	while (!sensor_matrix_get_in_ring(iteratorCount)
459
		&& iteratorCount < sensor_matrix_get_size()) {
460
		iteratorCount++;
461
	}
462

  
463
  //TODO: if you do the above comment, then you can compare this to the variable also
464
	if (iteratorCount == sensor_matrix_get_size()) {
465
		iteratorCount = -1;
466
	}
467

  
468
	return result;
469
}
470

  
471
/**
472
 * Returns the number of robots currently in the token ring.
473
 *
474
 * @return the number of robots in the token ring
475
 **/
476
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
477
// it reduces code size or not should be done to be sure.
478
int wl_token_get_num_robots(void)
479
{
480
	return sensor_matrix_get_joined();
481
}
482

  
483
/**
484
 * Returns the number of robots in the sensor matrix.
485
 *
486
 * @return the number of robots in the sensor matrix
487
 **/
488
//TODO: this function is so simple, it *may* be beneficial to inline this function.  testing of if
489
// it reduces code size or not should be done to be sure.
490
int wl_token_get_matrix_size(void)
491
{
492
	return sensor_matrix_get_size();
493
}
494

  
495
/**
337 496
 * This method is called when we receive a token pass packet.
497
 *
498
 * @param source the robot who passed the token to us.
499
 **/
500
static void wl_token_pass_receive(int source)
501
{
502
	WL_DEBUG_PRINT("Received token from ");
503
	WL_DEBUG_PRINT_INT(source);
504
	WL_DEBUG_PRINT(", expected ");
505
	WL_DEBUG_PRINT_INT(wl_token_next_robot);
506
	WL_DEBUG_PRINT(".\n");
507
	// this prevents two tokens from being passed around at a time (second clause is in case we are joining)
508
	if ((source != wl_token_next_robot && wl_get_xbee_id() != wl_token_next_robot) && bom_on_count <= DEATH_DELAY / 2 &&
509
		ringState != ACCEPTED)
510
	{
511
		WL_DEBUG_PRINT("Received token pass when a robot should not have died yet.\n");
512
		WL_DEBUG_PRINT("There are probably two tokens going around, packet ignored.\n");
513
		return;
514
	}
515
	bom_on_count = -1;
516
	deathDelay = -1;
517
	sensor_matrix_set_in_ring(source, 1);
518
	wl_token_get_token();
519
}
520

  
521
/**
522
 * This method is called when we receive a token pass packet.
338 523
 * @param source is the robot it came from
339 524
 * @param nextRobot is the robot the token was passed to
340 525
 * @param sensorData a char with an id followed by a char with the sensor
341 526
 *		reading for that robot, repeated for sensorDataLength bytes
342 527
 * @param sensorDataLength the length in bytes of sensorData
343 528
 */
344
void wl_token_pass_receive(int source, char nextRobot, unsigned char* sensorData, int sensorDataLength)
529
static void wl_token_sensor_matrix_receive(int source, unsigned char* sensorData, int sensorDataLength)
345 530
{
346 531
	int i, j;
532
	char nextRobot;
533

  
534
	bom_on_count = -1;
347 535
	deathDelay = -1;
536
	sensor_matrix_set_in_ring(source, 1);
348 537

  
349
	WL_DEBUG_PRINT("Received the token, next robot is ");
350
	WL_DEBUG_PRINT_INT((int)nextRobot);
351
	WL_DEBUG_PRINT(" \r\n");
352
	sensor_matrix_set_in_ring(sensorMatrix, source, 1);
353

  
354 538
	//with this packet, we are passed the id of the next robot in the ring
355 539
	//and the sensor matrix, a list of id and sensor reading pairs (two bytes for both)
356 540
	j = 0;
357
	for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)
541
  //TODO: the compiler may or may not optimize this such that my comment is useless:
542
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
543
  // the overhead of a function call each iteration, call it only once before the loop and store
544
  // the value in a variable and check against that variable in the loop condition
545
	for (i = 0; i < sensor_matrix_get_size(); i++)
358 546
	{
359
		if (i == source)
547
		if (i == source) {
360 548
			continue;
361
		
549
		}
550

  
362 551
		//set the sensor information we receive
363 552
		if (j < sensorDataLength / 2 && sensorData[2 * j] == i)
364 553
		{
......
368 557
				accepted = -1;
369 558
				WL_DEBUG_PRINT("Someone accepted the robot we did.\r\n");
370 559
			}
371
			sensor_matrix_set_reading(sensorMatrix, source, i,
560
			sensor_matrix_set_reading(source, i,
372 561
						sensorData[2 * j + 1]);
373
			sensor_matrix_set_in_ring(sensorMatrix, i, 1);
562
			if (!sensor_matrix_get_in_ring(i))
563
			{
564
				WL_DEBUG_PRINT("Robot ");
565
				WL_DEBUG_PRINT_INT(i);
566
				WL_DEBUG_PRINT(" has been added to the sensor matrix of robot ");
567
				WL_DEBUG_PRINT_INT(wl_get_xbee_id());
568
				WL_DEBUG_PRINT(" due to a packet from robot ");
569
				WL_DEBUG_PRINT_INT(source);
570
				WL_DEBUG_PRINT(".\r\n");
571
			}
572
			sensor_matrix_set_in_ring(i, 1);
374 573
			j++;
375 574
		}
376 575
		else
377 576
		{
378
			if (sensor_matrix_get_in_ring(sensorMatrix, i))
577
			if (sensor_matrix_get_in_ring(i))
379 578
			{
380 579
				WL_DEBUG_PRINT("Robot ");
381 580
				WL_DEBUG_PRINT_INT(i);
......
384 583
				WL_DEBUG_PRINT(" due to a packet from robot ");
385 584
				WL_DEBUG_PRINT_INT(source);
386 585
				WL_DEBUG_PRINT(".\r\n");
387
				sensor_matrix_set_in_ring(sensorMatrix, i, 0);
586
				sensor_matrix_set_in_ring(i, 0);
388 587
			}
389 588

  
390 589
			if (i == wl_get_xbee_id() && ringState == MEMBER)
391 590
			{
392 591
				ringState = NONMEMBER;
393 592
				wl_token_ring_join();
394
				
593

  
395 594
				WL_DEBUG_PRINT("We have been removed from the ring ");
396 595
				WL_DEBUG_PRINT("and are rejoining.\r\n");
397 596
			}
398
			
597

  
399 598
			//the person who accepted us is dead... let's ask again
400 599
			if (i == acceptor)
401 600
			{
402
				sensor_matrix_set_in_ring(sensorMatrix,
403
						wl_get_xbee_id(), 1);
601
				sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
404 602
				ringState = NONMEMBER;
405 603
				acceptor = -1;
406 604
				wl_token_ring_join();
407 605
			}
408 606
		}
409 607
	}
608
	
609
	// get the next robot in the token ring
610
	i = source + 1;
611
	while (1)
612
	{
613
		if (i == sensor_matrix_get_size()) {
614
			i = 0;
615
		}
410 616

  
411
	wl_token_next_robot = nextRobot;
412
	
617
		if (sensor_matrix_get_in_ring(i) || i == source)
618
		{
619
			nextRobot = (char)i;
620
			break;
621
		}
622

  
623
		i++;
624
	}
625

  
626
	if (nextRobot != wl_get_xbee_id())
627
		wl_token_next_robot = nextRobot;
628

  
413 629
	deathDelay = get_token_distance(wl_get_xbee_id(), nextRobot) * DEATH_DELAY;
414
	
415
	//we have the token
416
	if (wl_token_next_robot == wl_get_xbee_id())
417
		wl_token_get_token();
630

  
631
	if (sensor_matrix_get_joined() == 0 && ringState == JOINING)
632
		wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME);
418 633
}
419 634

  
420 635
/**
......
426 641
 * @return the number of passes before the token is expected
427 642
 * to reach robot2 from robot1
428 643
 **/
429
int get_token_distance(int robot1, int robot2)
644
static int get_token_distance(int robot1, int robot2)
430 645
{
431 646
	int curr = robot1 + 1;
432 647
	int count = 1;
433 648
	while (1)
434 649
	{
435
		if (curr == sensor_matrix_get_size(sensorMatrix))
650
    //TODO: the compiler may or may not optimize this such that my comment is useless:
651
    // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
652
    // the overhead of a function call each iteration, call it only once before the loop and store
653
    // the value in a variable and check against that variable in the loop condition
654
		if (curr == sensor_matrix_get_size())
436 655
			curr = 0;
437 656
		if (curr == robot2)
438 657
			break;
439
		if (sensor_matrix_get_in_ring(sensorMatrix, curr))
658
		if (sensor_matrix_get_in_ring(curr))
440 659
			count++;
441 660
		curr++;
442 661
	}
......
446 665
/**
447 666
 * Passes the token to the next robot in the token ring.
448 667
 **/
449
void wl_token_pass_token()
668
static int wl_token_pass_token()
450 669
{
451
	char nextRobot;
670
	char nextRobot = 0xFF;
452 671
	int i = wl_get_xbee_id() + 1;
672
	char buf[2 * sensor_matrix_get_size()];
453 673
	if (accepted == -1)
454 674
	{
455 675
		while (1)
456 676
		{
457
			if (i == sensor_matrix_get_size(sensorMatrix))
677
      //TODO: the compiler may or may not optimize this such that my comment is useless:
678
      // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
679
      // the overhead of a function call each iteration, call it only once before the loop and store
680
      // the value in a variable and check against that variable in the loop condition
681
			if (i == sensor_matrix_get_size()) {
458 682
				i = 0;
459
			if (sensor_matrix_get_in_ring(sensorMatrix, i))
683
			}
684

  
685
			if (sensor_matrix_get_in_ring(i))
460 686
			{
461 687
				nextRobot = (char)i;
462 688
				break;
463 689
			}
690

  
464 691
			i++;
465 692
		}
466 693
	}
......
468 695
	{
469 696
		WL_DEBUG_PRINT("Accepting new robot, sending it the token.\r\n");
470 697
		//add a new robot to the token ring
471
		sensor_matrix_set_in_ring(sensorMatrix, accepted, 1);
698
		sensor_matrix_set_in_ring(accepted, 1);
472 699
		nextRobot = accepted;
473 700
		accepted = -1;
474 701
	}
475 702

  
476
	//we don't include ourself
477
	int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);
478
	char* buf = (char*)malloc(packetSize * sizeof(char));
479
	if (!buf)
480
	{
481
		WL_DEBUG_PRINT_INT(packetSize);
482
		WL_DEBUG_PRINT("Out of memory - pass token.\r\n");
483
		return;
484
	}
485
	buf[0] = nextRobot;
486

  
487 703
	int j = 0;
488
	for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)
489
		if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id())
704
  //TODO: the compiler may or may not optimize this such that my comment is useless:
705
  // instead of calling sensor_matrix_get_size every iteration of the while loop and incurring
706
  // the overhead of a function call each iteration, call it only once before the loop and store
707
  // the value in a variable and check against that variable in the loop condition
708
	for (i = 0; i < sensor_matrix_get_size(); i++) {
709
		if (sensor_matrix_get_in_ring(i) && i != wl_get_xbee_id())
490 710
		{
491
			buf[2*j + 1] = i;
492
			buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);
711
			buf[2*j] = i;
712
			buf[2*j + 1] = sensor_matrix_get_reading(wl_get_xbee_id(), i);
493 713
			j++;
494 714
		}
495
	
715
	}
716

  
717
	int packetSize = 2 * j * sizeof(char);
496 718
	WL_DEBUG_PRINT("Passing the token to robot ");
497
	WL_DEBUG_PRINT_INT(buf[0]);
719
	WL_DEBUG_PRINT_INT(nextRobot);
498 720
	WL_DEBUG_PRINT(".\r\n");
499
	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS,
500
		buf, packetSize, 3);
721
	if (wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_SENSOR_MATRIX, buf, packetSize, 0) != 0)
722
		return -1;
723
	if (wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS, NULL, 0, nextRobot, WL_TOKEN_PASS_FRAME))
724
		return -1;
501 725

  
502 726
	wl_token_next_robot = nextRobot;
503 727
	deathDelay = DEATH_DELAY;
504
	free(buf);
728

  
729
	return 0;
505 730
}
506 731

  
507 732
/**
......
511 736
 *
512 737
 * @param source the robot whose BOM is on
513 738
 **/
514
void wl_token_bom_on_receive(int source)
739
static void wl_token_bom_on_receive(int source)
515 740
{
516 741
	WL_DEBUG_PRINT("Robot ");
517 742
	WL_DEBUG_PRINT_INT(source);
518 743
	WL_DEBUG_PRINT(" has flashed its bom.\r\n");
519
	sensor_matrix_set_reading(sensorMatrix, wl_get_xbee_id(), 
744

  
745
	bom_on_count = 0;
746

  
747
	sensor_matrix_set_reading(wl_get_xbee_id(),
520 748
		source, get_max_bom_function());
521 749
}
522 750

  
......
524 752
 * This method is called when we receive the token. Upon receiving
525 753
 * the token, we must send a BOM_ON packet, flash the BOM, and send
526 754
 * the token to the next robot.
527
 * 
755
 *
528 756
 * If there is a pending request for the token, this is processed first.
529 757
 **/
530
void wl_token_get_token()
758
static void wl_token_get_token()
531 759
{
532 760
	WL_DEBUG_PRINT("We have the token.\r\n");
533 761
	if (ringState == ACCEPTED)
534 762
	{
535
		sensor_matrix_set_in_ring(sensorMatrix,
536
			wl_get_xbee_id(), 1);
763
		sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
537 764
		WL_DEBUG_PRINT("Now a member of the token ring.\r\n");
538 765
		ringState = MEMBER;
766
		joinDelay = -1;
539 767
	}
540 768

  
541 769
	if (ringState == LEAVING || ringState == NONMEMBER)
542 770
	{
543
		sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 0);
771
		sensor_matrix_set_in_ring(wl_get_xbee_id(), 0);
544 772
		if (ringState == NONMEMBER)
545 773
		{
546 774
			WL_DEBUG_PRINT("We should have left the token ring, but didn't.\r\n");
547 775
		}
548 776
		return;
549 777
	}
550
	
551
	//check for interruption requests
552
	if (queue_size(interrupting) > 0)
553
	{
554
		char buf[1];
555
		buf[0] = (char)(int)queue_remove(interrupting);
556
		
557
		//in case this robot has requested multiple times
558
		queue_remove_all(interrupting, (void*)(int)buf[0]);
559 778

  
560
		wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_INTERRUPT_PASS,
561
			buf, 1, 0);
562

  
563
		deathDelay = DEATH_DELAY;
564
		wl_token_next_robot = buf[0];
565
		return;
566
	}
567

  
568 779
	WL_DEBUG_PRINT("Our BOM has been flashed.\r\n");
569
	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON,
570
		NULL, 0, 0);
780
	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON, NULL, 0, 0);
571 781

  
572 782
	bom_on_function();
573 783
	#ifdef ROBOT
574 784
	delay_ms(BOM_DELAY);
575 785
	#endif
576 786
	bom_off_function();
577
	
578
	if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))
787

  
788
	if (!sensor_matrix_get_in_ring(wl_get_xbee_id()))
579 789
	{
580 790
		WL_DEBUG_PRINT("Removed from sensor matrix while flashing BOM.\r\n");
791
		return;
581 792
	}
582
	
793

  
583 794
	wl_token_pass_token();
584 795
}
585 796

  
......
591 802
 *
592 803
 * @param source the robot who requested to join
593 804
 **/
594
void wl_token_join_receive(int source)
805
static void wl_token_join_receive(int source)
595 806
{
596 807
	WL_DEBUG_PRINT("Received joining request from robot ");
597 808
	WL_DEBUG_PRINT_INT(source);
......
606 817
	//we can only accept one request at a time
607 818
	if (accepted != -1)
608 819
		return;
609
	
820

  
610 821
	//check if we are the preceding robot in the token ring
611 822
	int i = source - 1;
612 823
	while (1)
613 824
	{
614 825
		if (i < 0)
615
			i = sensor_matrix_get_size(sensorMatrix) - 1;
826
			i = sensor_matrix_get_size() - 1;
827

  
616 828
		//we must send a join acceptance
617 829
		if (i == wl_get_xbee_id())
618 830
			break;
619 831

  
620 832
		//another robot will handle it
621
		if (sensor_matrix_get_in_ring(sensorMatrix, i))
833
		if (sensor_matrix_get_in_ring(i))
622 834
			return;
835

  
623 836
		i--;
624 837
	}
625 838

  
626 839
	accepted = source;
627 840
	wl_send_robot_to_robot_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_JOIN_ACCEPT,
628 841
		NULL, 0, source, TOKEN_JOIN_ACCEPT_FRAME);
629
	
842

  
630 843
	WL_DEBUG_PRINT("Accepting robot ");
631 844
	WL_DEBUG_PRINT_INT(source);
632 845
	WL_DEBUG_PRINT(" into the token ring.\r\n");
633 846

  
634
	joinDelay = -1;
635
	
636 847
	// the token ring has not started yet
637
	if (sensor_matrix_get_joined(sensorMatrix) == 1)
848
	if (sensor_matrix_get_joined() == 1)
638 849
		wl_token_pass_token();
639 850
}
640 851

  
......
645 856
 *
646 857
 * @param source the robot who accepted us
647 858
 **/
648
void wl_token_join_accept_receive(int source)
859
static void wl_token_join_accept_receive(int source)
649 860
{
650 861
	WL_DEBUG_PRINT("Accepted into the token ring by robot ");
651 862
	WL_DEBUG_PRINT_INT(source);
652 863
	WL_DEBUG_PRINT(".\r\n");
653
	joinDelay = -1;
864
	joinDelay = JOIN_DELAY;
654 865
	ringState = ACCEPTED;
655 866
	acceptor = source;
656 867

  
657 868
	//add ourselves to the token ring
658
	sensor_matrix_set_in_ring(sensorMatrix, wl_get_xbee_id(), 1);
869
	sensor_matrix_set_in_ring(wl_get_xbee_id(), 1);
659 870
}
660

  
661
/**
662
 * Called when we receive a packet passing the token and interrupting
663
 * the token ring.
664
 * If the token has been passed to us, we flash our BOM
665
 * and pass it back.
666
 *
667
 * @param source the robot who sent the interrupt packet
668
 * @param robot the robot the token has been passed to
669
 **/
670
void wl_token_interrupt_pass_receive(int source, int robot)
671
{
672
	if (wl_get_xbee_id() != robot)
673
	{
674
		queue_remove_all(interrupting, (void*)robot);
675
		wl_token_next_robot = robot;
676
		deathDelay = DEATH_DELAY + rand() / (RAND_MAX / (2 * DEATH_DELAY));
677
		return;
678
	}
679
	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_BOM_ON,
680
		NULL, 0, 0);
681
	
682
	bom_on_function();
683
	#ifdef ROBOT
684
	delay_ms(BOM_DELAY);
685
	#endif
686
	bom_off_function();
687

  
688
	//we don't include ourself, only if we are in the ring
689
	int packetSize = 1 + 2 * (sensor_matrix_get_joined(sensorMatrix) - 1);
690
	if (!sensor_matrix_get_in_ring(sensorMatrix, wl_get_xbee_id()))
691
		packetSize += 2;
692
	char* buf = (char*)malloc(packetSize * sizeof(char));
693
	if (!buf)
694
	{
695
		WL_DEBUG_PRINT("Out of memory - pass_receive.\r\n");
696
		return;
697
	}
698
	
699
	//return the token to where we got it from
700
	buf[0] = source;
701

  
702
	int i = 0, j = 0;
703
	for (i = 0; i < sensor_matrix_get_size(sensorMatrix); i++)
704
		if (sensor_matrix_get_in_ring(sensorMatrix, i) && i != wl_get_xbee_id())
705
		{
706
			buf[2*j + 1] = i;
707
			buf[2*j + 2] = sensor_matrix_get_reading(sensorMatrix, wl_get_xbee_id(), i);
708
			j++;
709
		}
710
	
711
	wl_send_global_packet(WL_TOKEN_RING_GROUP, WL_TOKEN_PASS,
712
		buf, packetSize, 0);
713

  
714
	wl_token_next_robot = source;
715
	deathDelay = DEATH_DELAY;
716
	free(buf);
717
}
718

  
719
/**
720
 * Returns the number of robots in the token ring.
721
 *
722
 * @return the number of robots in the token ring
723
 **/
724
int wl_token_get_robots_in_ring(void)
725
{
726
	return sensor_matrix_get_joined(sensorMatrix);
727
}
728

  
729
/**
730
 * Returns true if the specified robot is in the token ring, false
731
 * otherwise.
732
 *
733
 * @param robot the robot to check for whether it is in the token ring
734
 * @return nonzero if the robot is in the token ring, zero otherwise
735
 **/
736
int wl_token_is_robot_in_ring(int robot)
737
{
738
	return sensor_matrix_get_in_ring(sensorMatrix, robot);
739
}
740

  
741
/**
742
 * Begins iterating through the robots in the token ring.
743
 *
744
 * @see wl_token_iterator_has_next, wl_token_iterator_next
745
 **/
746
void wl_token_iterator_begin(void)
747
{
748
	int i = 0;
749
	iteratorCount = 0;
750
	while (!sensor_matrix_get_in_ring(sensorMatrix, i) &&
751
			i < sensor_matrix_get_size(sensorMatrix))
752
		i++;
753
	if (i == sensor_matrix_get_size(sensorMatrix))
754
		i = -1;
755
}
756

  
757
/**
758
 * Returns true if there are more robots in the token ring
759
 * to iterate through, and false otherwise.
760
 *
761
 * @return nonzero if there are more robots to iterate through,
762
 * zero otherwise
763
 *
764
 * @see wl_token_iterator_begin, wl_token_iterator_next
765
 **/
766
int wl_token_iterator_has_next(void)
767
{
768
	return iteratorCount == -1;
769
}
770

  
771
/**
772
 * Returns the next robot ID in the token ring.
773
 *
774
 * @return the next robot ID in the token ring, or -1 if none exists
775
 *
776
 * @see wl_token_iterator_begin, wl_token_iterator_has_next
777
 **/
778
int wl_token_iterator_next(void)
779
{
780
	int result = iteratorCount;
781
	if (result < 0)
782
		return result;
783

  
784
	iteratorCount++;
785
	while (!sensor_matrix_get_in_ring(sensorMatrix, iteratorCount) &&
786
		iteratorCount < sensor_matrix_get_size(sensorMatrix))
787
		iteratorCount++;
788
	if (iteratorCount == sensor_matrix_get_size(sensorMatrix))
789
		iteratorCount = -1;
790
	return result;
791
}
792

  
793
/**
794
 * Called when we receive a request to interrupt the token ring.
795
 * We add the robot to our list of interrupt requests,
796
 * and will send the token to this robot when we next receive the
797
 * token, unless someone else does so first.
798
 *
799
 * @param source the robot requesting interruption
800
 * @param robt the robot requested to interrupt the token ring
801
 **/
802
void wl_token_interrupt_request_receive(int source, int robot)
803
{
804
	queue_add(interrupting, (void*)robot);
805
}
806

  
807
int wl_token_get_num_robots(void){
808
  return sensor_matrix_get_joined(sensorMatrix);
809
}
810

  
811
int wl_token_get_matrix_size(void){
812
  return sensor_matrix_get_size(sensorMatrix);
813
}

Also available in: Unified diff