Project

General

Profile

Revision 1450

Added by James Kong about 7 years ago

branched analog

View differences:

branches/analog/code/projects/hunter_prey/testbench/main.c
1
/* testbench for Lab 2 first checkpoint
2
 * determine whether robot under test complies with communication standard
3
 * to be conducted using XBee USB dongle
4
 */
5

  
6
/* The tests shall be as follows
7
 * 1. Receive a TAG, send an ACK
8
 * - Robot passes if it sends a tag and changes to prey state
9
 * 2. Send a TAG, receive an ACK
10
 * - Pass if sends ACK, changes to wait state, then hunter state
11
 * 3. Receive TAG, send slightly delayed (< 1s) ACK
12
 * - Pass if sends one and only one TAG packet (and changes state appropriately)
13
 * 4. Receive TAG, do not send ACK
14
 * - Pass if sends one and only one TAG packet (and does not change state)
15
 * 5. Receive TAG, send ACK to incorrect robot
16
 * - Pass if goes to wait state, then back to hunter state
17
 * 6. Simulate TAG from robot A to B, ACK from B to A
18
 * - Pass if ignores TAG, goes to wait then hunter in response to ACK
19
 */
20

  
21
#include <stdlib.h>
22
#include <stdio.h>
23
#include <unistd.h>
24
#include "../../libwireless/lib/wl_basic.h"
25
#include "../../libwireless/lib/wireless.h"
26
#include "hunter_prey.h"
27
#include <time.h>
28

  
29
#define CHANNEL 0xF // channel for wireless communication
30
#define TYPE 42	// packet type for wireless communication
31
#define ROBOTID 255 // make up a robot id because the PC doesn't have one
32

  
33
int main(int argc, char *argv[])
34
{
35
    char send_buffer[2];    // holds data to send
36
    int data_length;	// length of data received
37
    unsigned char *packet_data;	// data received
38
    int ret;
39

  
40
    struct timespec delay8, rem;
41

  
42
    delay8.tv_sec = 2;
43
    delay8.tv_nsec = 800000000;
44

  
45
    wl_set_com_port("/dev/ttyUSB0");
46
    // set up wireless
47
    wl_basic_init_default();
48
    wl_set_channel(CHANNEL);
49

  
50
    printf("Testing communications\n\n");
51

  
52
    // Receive TAG, send ACK
53
    printf("Receive TAG, send ACK... ");
54
    fflush(stdout);
55
    // Wait until we receive a packet
56
    while (!(packet_data = wl_basic_do_default(&data_length)));
57

  
58
    printf("got packet, validating and sending ack...");
59
    fflush(stdout);
60

  
61
    if (data_length > 2)
62
    {
63
	printf("Excessive TAG packet length... ");
64
	fflush(stdout);
65
    }
66

  
67
    if (data_length >= 2 && packet_data[0] == HUNTER_PREY_ACTION_TAG)
68
    {
69
	// send back an ACK
70
	send_buffer[0] = HUNTER_PREY_ACTION_ACK;
71
	send_buffer[1] = packet_data[1];
72
	printf("sending ack intended for robot %d\n", packet_data[1]);
73
	fflush(stdout);
74
	wl_basic_send_global_packet(TYPE, send_buffer, 2);
75

  
76
	printf("PASSED\n");
77
    }
78
    else
79
	printf("FAILED\n");
80

  
81

  
82
    sleep(10);
83

  
84
    // Send a TAG, receive an ACK
85
    printf("Send TAG, wait for ACK... ");
86
    fflush(stdout);
87

  
88
    send_buffer[0] = HUNTER_PREY_ACTION_TAG;
89
    send_buffer[1] = ROBOTID;
90
    // robot number stays the same
91
    wl_basic_send_global_packet(TYPE, send_buffer, 2);
92

  
93
    // Wait for an ACK
94
    ret = nanosleep(&delay8, &rem);  // wait for 800 ms before sending ACK
95

  
96
    while(ret) {
97
      //      printf("nanosleep not done, needs to wait %ld more\n", rem.tv_nsec);
98
      ret = nanosleep(&rem, &rem);
99
    }
100

  
101
    data_length = -1;
102
    packet_data = wl_basic_do_default(&data_length);
103

  
104
    // Check ACK for presence and correctness
105
    if (!packet_data || data_length < 2 ||
106
	    packet_data[0] != HUNTER_PREY_ACTION_ACK ||
107
	    packet_data[1] != ROBOTID)
108
      printf("FAILED, packet=%p, len=%d\n", packet_data, data_length);
109
    else
110
	printf("PASSED\n");
111

  
112
    // Receive a TAG, send a delayed ACK
113
    printf("Receive a TAG, send a delayed ACK... ");
114
    fflush(stdout);
115

  
116
    while (!(packet_data = wl_basic_do_default(&data_length)));
117

  
118
    if (data_length >= 2 && packet_data[0] == HUNTER_PREY_ACTION_TAG)
119
    {
120
        printf("got TAG, waiting...");
121
	fflush(stdout);
122
	// wait before sending ACK back
123
	//usleep(900000);
124
        delay8.tv_sec = 0;
125
        ret = nanosleep(&delay8, &rem);  // wait for 800 ms before sending ACK
126

  
127
	while(ret) {
128
	  printf(".");
129
	  fflush(stdout);//      printf("nanosleep not done, needs to wait %ld more\n", rem.tv_nsec);
130
	  ret = nanosleep(&rem, &rem);
131
	}
132

  
133

  
134
	if (wl_basic_do_default(&data_length))
135
	    printf("FAILED, got another TAG too soon\n");
136
	else
137
	{
138
	    // send packet
139
	    send_buffer[0] = HUNTER_PREY_ACTION_ACK;
140
	    send_buffer[1] = packet_data[1];
141
	    wl_basic_send_global_packet(TYPE, send_buffer, 2);
142

  
143
	    printf("PASSED\n");
144
	}
145
    }
146

  
147
    else
148
	printf("FAILED\n");
149

  
150

  
151
    printf("sending courtesy tag...");
152
    fflush(stdout);
153
    send_buffer[0] = HUNTER_PREY_ACTION_TAG;
154
    send_buffer[1] = ROBOTID;
155
    // robot number stays the same
156
    wl_basic_send_global_packet(TYPE, send_buffer, 2);
157
    printf("done.\nwaiting for ack...");
158
    fflush(stdout);
159

  
160
    while (!(packet_data = wl_basic_do_default(&data_length)));
161

  
162
    if (!packet_data || data_length < 2 ||
163
	    packet_data[0] != HUNTER_PREY_ACTION_ACK ||
164
	    packet_data[1] != ROBOTID)
165
      printf("FAILED, packet=%p, len=%d\n", packet_data, data_length);
166
    else
167
	printf("done\n");
168

  
169

  
170
    // Receive a TAG, never send an ACK
171
    printf("Receive TAG, never send ACK... ");
172
    fflush(stdout);
173

  
174
    while (!(packet_data = wl_basic_do_default(&data_length)));
175

  
176
    if (data_length >= 2 && packet_data[0] == HUNTER_PREY_ACTION_TAG)
177
    {
178

  
179
        printf("got TAG, monitoring for 5 seconds\n");
180
	fflush(stdout);
181
	usleep(5000000);    // wait 5 seconds to see if they TAG again
182

  
183
	if (wl_basic_do_default(&data_length))
184
	    printf("FAILED, robot is spamming\n");
185
	else
186
	    printf("PASSED\n");
187
    }
188

  
189
    else
190
	printf("FAILED\n");
191

  
192
    // Receive TAG, send ACK to incorrect robot
193
    printf("Receive TAG, send ACK to incorrect robot... ");
194
    fflush(stdout);
195

  
196
    // Wait until we receive a packet
197
    while (!(packet_data = wl_basic_do_default(&data_length)));
198

  
199
    if (data_length >= 2 && packet_data[0] == HUNTER_PREY_ACTION_TAG)
200
    {
201
	// send back an ACK
202
	send_buffer[0] = HUNTER_PREY_ACTION_ACK;
203
	send_buffer[1] = packet_data[1] + 1;
204
	wl_basic_send_global_packet(TYPE, send_buffer, 2);
205

  
206
	printf("PASSED\n");
207
    }
208
    else
209
	printf("FAILED\n");
210

  
211
    // Simulate TAG from robot A to B, ACK from robot B to A
212
    printf("Send TAG from robot A to B, ACK from B to A... ");
213
    fflush(stdout);
214

  
215
    // TAG
216
    send_buffer[0] = HUNTER_PREY_ACTION_TAG;
217
    send_buffer[1] = packet_data[1] - 1;    // robot other than testee
218
    wl_basic_send_global_packet(TYPE, send_buffer, 2);
219

  
220
    // ACK
221
    send_buffer[0] = HUNTER_PREY_ACTION_ACK;
222
    // send_buffer[1] stays the same
223
    wl_basic_send_global_packet(TYPE, send_buffer, 2);
224

  
225
    if (wl_basic_do_default(&data_length))
226
	printf("FAILED\n");
227
    else
228
	printf("PASSED\n");
229

  
230
    return 0;
231
}
232

  
branches/analog/code/projects/hunter_prey/testbench/Makefile
1
CC = gcc
2
CFLAGS = -Wall -g
3

  
4
all: testbench
5

  
6
testbench: *.c ../../libwireless/lib/*.c 
7
	$(CC) $(CFLAGS) *.c -L../../libwireless/lib -o testbench -DWL_DEBUG -lwireless -lpthread -ggdb
8

  
9
clean:
10
	rm -f *.o testbench ../lib/*.o
11

  
branches/analog/code/projects/hunter_prey/testbench/hunter_prey.h
1
#ifndef _HUNTER_PREY_H
2
#define _HUNTER_PREY_H
3

  
4
#include <inttypes.h>
5

  
6
/*
7
 * The packet structure is 2 bytes
8
 * byte 0 is the action, which is one of the values below
9
 * byte 1 is the robot id
10
 */
11

  
12
#define HUNTER_PREY_ACTION_TAG 'T'
13
#define HUNTER_PREY_ACTION_ACK 'A'
14

  
15
uint8_t hunter_prey_tagged(int max_bom, int front_rangefinder);
16

  
17
#endif
branches/analog/code/projects/hunter_prey/main.c
1
#include <dragonfly_lib.h>
2
#include <wireless.h>
3
#include <wl_basic.h>
4
#include "smart_run_around_fsm.h"
5
#include "hunter_prey.h"
6

  
7
#define CHAN 0xF
8
#define TAG_PAUSE 2000
9

  
10
#define ROBOT_HUNTER 0
11
#define ROBOT_PREY 1
12
#define ROBOT_TAGGED 2
13

  
14
void packet_receive(char type, int source, unsigned char* packet, int length);
15

  
16
volatile uint8_t robotState = ROBOT_HUNTER;
17
volatile uint8_t color1, color2;
18
volatile uint8_t tagAck;
19
volatile uint8_t tagger;
20
uint8_t id;
21

  
22
void test_bom(void) {
23
  int i;
24
  int val;
25

  
26
  while(1) {
27
    bom_refresh(BOM_ALL);
28

  
29
    for(i=0; i<16; i++) {
30
      val = bom_get(i);
31
      usb_puti(val);
32
      usb_putc(' ');
33
    }
34

  
35
    usb_puts("| ");
36

  
37
    usb_puti(range_read_distance (IR1));
38
    usb_putc(' ');
39
    usb_puti(range_read_distance (IR2));
40
    usb_putc(' ');
41
    usb_puti(range_read_distance (IR3));
42
    usb_putc(' ');
43
    usb_puti(range_read_distance (IR4));
44
    usb_putc(' ');
45
    usb_puti(range_read_distance (IR5));
46

  
47
    usb_putc('\n');
48
    delay_ms(500);
49
  }
50

  
51
}
52

  
53
void chase(uint8_t angle) {
54

  
55
  int omega = angle - 4;
56

  
57
  if(angle > 16)
58
    return;
59

  
60
  if(omega >  8)
61
    omega = omega - 16;
62

  
63
  omega *= 255/8;
64

  
65
/*   usb_puti(omega); */
66
/*   usb_putc('\n'); */
67

  
68
  move(140, omega);
69

  
70
}
71

  
72
int main(void) {
73

  
74
  int frontRange;
75
  uint8_t angle;
76
  unsigned char packet[2];
77
  
78
  //intialize the robot and wireless
79
  dragonfly_init(ALL_ON);
80

  
81
  //  test_bom();
82

  
83
  id = get_robotid();
84
  packet[1] = id;
85
  usb_puts("hello, I am robot \n");
86
  usb_puti(id);
87
  usb_putc('\n');
88

  
89
  usb_puts("wireless.\n");
90

  
91
  wl_basic_init(&packet_receive);
92

  
93
  usb_puts("run around.\n");
94

  
95
  run_around_init();
96

  
97
  usb_puts("channel\n");
98

  
99
  //use a specific channel to avoid interfering with other tasks
100
  wl_set_channel(CHAN);
101

  
102
  usb_puts("orb set\n");
103

  
104
  orb_set_color(BLUE);
105
  color1 = BLUE;
106
  color2 = BLUE;
107

  
108
  //if button 2 is held, you are prey
109
  if(button2_read()) {
110
    robotState = ROBOT_PREY;
111
    bom_on();
112
  }
113

  
114

  
115
  while(1) {
116

  
117
    if(robotState==ROBOT_TAGGED) {
118
      usb_puts("tagged, waiting to send ACK\n");
119
      //delay_ms(TAG_PAUSE);
120
      move(0,0);
121
      usb_puts("sending ACK to ");
122
      usb_puti(tagger);
123
      usb_putc('\n');
124
      packet[0] = HUNTER_PREY_ACTION_ACK;
125
      packet[1] = tagger;
126
      wl_basic_send_global_packet (42, &packet, 2);
127
      usb_puts("sent\n");
128
      robotState = ROBOT_HUNTER; //no longer prey
129
    }
130

  
131
    else if(robotState == ROBOT_PREY) {
132
      run_around_FSM();
133
    }
134

  
135
    else { //ROBOT_HUNTER
136

  
137
      bom_refresh(BOM_ALL);
138

  
139
      delay_ms(100);
140

  
141
      frontRange = range_read_distance (IR2);
142

  
143
/*       usb_puti(frontRange); */
144
/*       usb_putc(','); */
145

  
146
      //BUG: when get_max_bom is called, the rangefinder seems to stop
147
      //working
148
      angle = bom_get_max();
149
/*       usb_puti(angle); */
150
/*       usb_putc('\n'); */
151

  
152
      chase(angle);
153

  
154
      //debugging to determine tagging conditions
155
      if(angle < 7 && angle > 1) {
156
	if(color1 != RED) {
157
	  color1 = RED;
158
	  orb1_set_color(RED);
159
	}
160
      }
161
      else {
162
	if(color1 != BLUE) {
163
	  color1 = BLUE;
164
	  orb1_set_color(BLUE);
165
	}
166
      }
167

  
168

  
169
      usb_puti(frontRange);
170
      usb_putc('\n');
171

  
172
/*       if(frontRange > 0 && frontRange < TAG_RANGE) { */
173
/* 	if(color2 != RED) { */
174
/* 	  color2 = RED; */
175
/* 	  orb2_set_color(RED); */
176
/* 	} */
177
/*       } */
178
/*       else { */
179
/* 	if(color2 != BLUE) { */
180
/* 	  color2 = BLUE; */
181
/* 	  orb2_set_color(BLUE); */
182
/* 	} */
183
/*       } */
184

  
185

  
186

  
187
      if(hunter_prey_tagged(angle, frontRange) || button1_read()) {
188
	orb_set_color(YELLOW);
189
	color1 = YELLOW;
190
	color2 = YELLOW;
191
	usb_puts("TAG!\n");
192
	tagAck = 0;
193
	packet[0] = HUNTER_PREY_ACTION_TAG;
194
	packet[1] = id;
195
	wl_basic_send_global_packet (42, &packet, 2);
196
	move(0,0);
197
	while(!tagAck)
198
	  wl_do();
199

  
200
	if(tagAck == id) { //if the ack was for us
201
	  usb_puts("ACK!\n");
202
	      
203
	  move(-230,0); //back up to give the new prey some room
204
	  delay_ms(TAG_PAUSE/2);
205
	  move(0,0);
206

  
207
	  robotState = ROBOT_PREY;
208
	  bom_on();
209
	}
210
	else {
211
	  usb_puts("ACK failed!\n");
212
	}
213
      }
214
    }
215

  
216
    wl_do();
217
  }
218
  
219
  return 0;
220
}
221

  
222
void packet_receive(char type, int source, unsigned char* packet, int length)
223
{
224

  
225
  if(type==42 && length>=2){
226
    if(packet[0] == HUNTER_PREY_ACTION_TAG) {
227
      if(robotState == ROBOT_PREY) {
228
	robotState = ROBOT_TAGGED;
229
	bom_off();
230
	color1 = BLUE;
231
	color2 = BLUE;
232
	orb_set_color(BLUE);
233
	tagger = packet[1];
234
      }
235
      else {
236
	usb_puts("tagged, but I'm not it!\n");
237
      }
238
    }
239
    else if(packet[0] == HUNTER_PREY_ACTION_ACK && robotState == ROBOT_HUNTER) {
240
      tagAck = packet[1];
241
    }
242
    else {
243
      usb_puts("got '");
244
      usb_putc(packet[0]);
245
      usb_puti(packet[1]);
246
      usb_puts("'\n");
247
    }
248
  }
249
  else {
250
    usb_puts("got unkown packet! type=");
251
    usb_puti(type);
252
    usb_puts(" length=");
253
    usb_puti(length);
254
    usb_putc('\n');
255
  }
256
}
257

  
258
#define HP_BUF_IDX_ACTION 0
branches/analog/code/projects/hunter_prey/smart_run_around_fsm.c
1
#include <dragonfly_lib.h>
2
#include "smart_run_around_fsm.h"
3

  
4
void run_around_init(void)
5
{
6
  range_init();
7
  orb_init();
8
 
9
  state = SRA_FORWARD;  // start out moving forward
10
  // initialize rangefinder values to 0
11
  d1 = 0, d2 = 0, d3 = 0, d4 = 0, d5 = 0;
12
}
13

  
14
/* evaluate rangefinder input and update state
15
 * call this function as often as possible to avoid collisions
16
 */
17
void run_around_FSM(void)
18
{
19
    /* TODO: find a better way to handle rangefinder input
20
     * robot should deal with -1s (obstacles too close or too far to detect)
21
     * in a way that keeps it from crashing or driving in circles
22
     */
23
    // do not update distances when rangefinders return -1
24
    // otherwise update distances with new rangefinder values
25
    int16_t temp;
26

  
27
    temp = range_read_distance(IR1);
28
    d1 = (temp == -1) ? d1 : temp;
29

  
30
    temp = range_read_distance(IR2);
31
    d2 = (temp == -1) ? d2 : temp;
32

  
33
    temp = range_read_distance(IR3);
34
    d3 = (temp == -1) ? d3 : temp;
35

  
36
    temp = range_read_distance(IR4);
37
    d4 = (temp == -1) ? d4 : temp;
38

  
39
    temp = range_read_distance(IR5);
40
    d5 = (temp == -1) ? d5 : temp;
41

  
42
    // update state based on rangefinder input
43
    if (d2 < IR2_DANGER)    // avoid frontal collision by turning in place
44
	state = SRA_SPIN;
45
    /* nowhere to turn, so don't
46
     * will probably need to turn around soon, but not yet
47
     */
48
    else if (d1 < IR13_DANGER && d3 < IR13_DANGER)
49
	state = SRA_FORWARD;
50
    // avoid left-side collision by turning right
51
    else if (d1 < IR13_DANGER || d5 < IR45_DANGER)
52
	state = SRA_RIGHT;
53
    // avoid right-side collision by turning left
54
    else if (d3 < IR13_DANGER || d4 < IR45_DANGER)
55
	state = SRA_LEFT;
56
    else if (d2 < IR2_INTEREST)	// should turn to avoid obstacle up ahead
57
    {
58
	if (d3 >= d1)	// more room on right
59
	    state = SRA_RIGHT;
60
	else	// more room on left
61
	    state = SRA_LEFT;
62
    }
63
    else    // no obstacles close by, so keep going straight
64
	state = SRA_FORWARD;
65

  
66
    /* Debugging via USB output */
67
/*     usb_puts("IR1: "); */
68
/*     usb_puti(d1); */
69
/*     usb_puts(" IR2: "); */
70
/*     usb_puti(d2); */
71
/*     usb_puts(" IR3: "); */
72
/*     usb_puti(d3); */
73
/*     usb_puts(" IR4: "); */
74
/*     usb_puti(d4); */
75
/*     usb_puts(" IR5: "); */
76
/*     usb_puti(d5); */
77
/*     usb_puts("\n\r"); */
78

  
79
    evaluate_state();	// take action on updated state
80
}
81

  
82
// behave according to current state
83
// TODO: adjust speeds after testing
84
void evaluate_state(void)
85
{
86
    switch (state)
87
    {
88
	case SRA_FORWARD:
89
	    orb_set_color(GREEN);
90
	    move(220, 0);	// drive straight forward
91
	    break;
92
	case SRA_LEFT:
93
	    orbs_set_color(GREEN, YELLOW);  // green on left side
94
	    move(HALF_SPD, NRML_TURN);	// drive forward and to the left
95
	    break;
96
	case SRA_RIGHT:
97
	    orbs_set_color(YELLOW, GREEN);  // green on right side
98
	    move(HALF_SPD, -NRML_TURN);	// drive forward and to the right
99
	    break;
100
	case SRA_SPIN:
101
	    orb_set_color(RED);
102
	    move(0, NRML_TURN);	// spin CCW without traveling
103
	    break;
104
	default:    // should never reach this
105
	    orb_set_color(PURPLE);
106
	    move(0, 0);	// stop completely
107
    }
108
}
109

  
0 110

  
branches/analog/code/projects/hunter_prey/hunter_prey.c
1
#include "hunter_prey.h"
2
#include <dragonfly_lib.h>
3

  
4
#define TAG_TIME 3
5
#define TAG_RANGE 200
6

  
7
/**** This file should not be edited! ****/
8

  
9

  
10
/*
11
 * The criteria for tagging are the following:
12
 *   * The max bom is betweed 1 and 7
13
 *   * The front rangefinder reads less than TAG_RANGE
14
 *   * -1 values from the rangefinder are ignored
15
 *   * These conditions are met across TAG_TIME calls to this function
16
 */
17

  
18
uint8_t hunter_prey_tagged(int max_bom, int frontRange) {
19

  
20
  static int onTarget = 0;
21

  
22
  if(max_bom < 7 && max_bom > 1 && frontRange > 0 && frontRange < TAG_RANGE) {
23
    if(onTarget == 0) {
24
      onTarget = TAG_TIME;
25
      usb_puts("On target!\n");
26
    }
27
    else {
28
      if(--onTarget <= 0) {
29
	onTarget = 0;
30
	usb_puts("TAG!\n");
31
	return 1;
32
      }
33
    }
34
  }
35
  else{
36
    //don't reset onTarget because the robot got too close
37
    if(frontRange > 0)
38
      onTarget = 0;
39
  }
40

  
41
  return 0;
42

  
43
}
branches/analog/code/projects/hunter_prey/hunter_prey.h
1
#ifndef _HUNTER_PREY_H
2
#define _HUNTER_PREY_H
3

  
4
#include <inttypes.h>
5

  
6
/*
7
 * The packet structure is 2 bytes
8
 * byte 0 is the action, which is one of the values below
9
 * byte 1 is the robot id
10
 */
11

  
12
#define HUNTER_PREY_ACTION_TAG 'T'
13
#define HUNTER_PREY_ACTION_ACK 'A'
14

  
15
uint8_t hunter_prey_tagged(int max_bom, int front_rangefinder);
16

  
17
#endif
branches/analog/code/projects/hunter_prey/smart_run_around_fsm.h
1
// smart_run_around_fsm.h
2
// required to run Smart Run Around functions
3
// declare functions and global variables
4

  
5
#ifndef _RUN_AROUND_FSM_H_
6
#define _RUN_AROUND_FSM_H_
7

  
8
// states (robot shall never move in reverse)
9
// these constants may be changed (but can't overlap) without effect
10
#define SRA_FORWARD 0   // drive straight forward
11
#define SRA_LEFT 1	// drive forward and to the left
12
#define SRA_RIGHT 2	// drive forward and to the right
13
#define SRA_SPIN 3	// turn without traveling (last resort if otherwise stuck)
14

  
15
/* conditions on rangefinders (in mm) informing state changes
16
 * distances are 50 greater than in real life, cannot be smaller than 100
17
 * naming format: SENSOR_URGENCY
18
 * changing these constants affects robot operation
19
 */
20
#define IR2_DANGER 150	// 10 cm: dangerously close to front
21
#define IR13_DANGER 200 // 15 cm: dangerously close to sides (looking ahead)
22
#define IR2_INTEREST 300    // 25 cm: notably close to front
23
#define IR45_DANGER 150	// dangerously close to side (looking sideways)
24

  
25
uint8_t state;	// current state of FSM: SRA_FORWARD, SRA_LEFT, SRA_RIGHT, SRA_SPIN
26
int16_t d1, d2, d3, d4, d5; // values returned by rangefinders
27

  
28
// initialize rangefinders and global variables
29
void run_around_init(void);
30

  
31
/* evaluate rangefinder input and update state
32
 * call this function as often as possible to avoid collisions
33
 */
34
void run_around_FSM(void);
35

  
36
// behave according to current state
37
void evaluate_state(void);
38

  
39
#endif
40

  
0 41

  
branches/analog/code/projects/hunter_prey/Makefile
1
########Update This Section########
2
#
3
#
4

  
5
# Relative path to the root directory (containing lib directory)
6
ifndef COLONYROOT
7
COLONYROOT = ../../..
8
endif
9

  
10
# Target file name (without extension).
11
TARGET = main
12

  
13
# Uncomment this to use the wireless library
14
USE_WIRELESS = 1
15

  
16
# com1 = serial port. Use lpt1 to connect to parallel port.
17
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
18

  
19
#
20
###################################
21

  
22
# Hey Emacs, this is a -*- makefile -*-
23
#----------------------------------------------------------------------------
24
# WinAVR Makefile Template written by Eric B. Weddington, J?rg Wunsch, et al.
25
#
26
# Released to the Public Domain
27
#
28
# Additional material for this makefile was written by:
29
# Peter Fleury
30
# Tim Henigan
31
# Colin O'Flynn
32
# Reiner Patommel
33
# Markus Pfaff
34
# Sander Pool
35
# Frederik Rouleau
36
#
37
#----------------------------------------------------------------------------
38
# On command line:
39
#
40
# make all = Make software.
41
#
42
# make clean = Clean out built project files.
43
#
44
# make coff = Convert ELF to AVR COFF.
45
#
46
# make extcoff = Convert ELF to AVR Extended COFF.
47
#
48
# make program = Download the hex file to the device, using avrdude.
49
#                Please customize the avrdude settings below first!
50
#
51
# make debug = Start either simulavr or avarice as specified for debugging,
52
#              with avr-gdb or avr-insight as the front end for debugging.
53
#
54
# make filename.s = Just compile filename.c into the assembler code only.
55
#
56
# make filename.i = Create a preprocessed source file for use in submitting
57
#                   bug reports to the GCC project.
58
#
59
# To rebuild project do "make clean" then "make all".
60
#----------------------------------------------------------------------------
61

  
62
#if you want your code to work on the Firefly++ and not Firefly+
63
#then add the -DFFPP line to CDEFS
64
CDEFS =
65
#-DFFPP
66

  
67
# MCU name
68
MCU = atmega128
69

  
70
# Processor frequency.
71
#     This will define a symbol, F_CPU, in all source code files equal to the
72
#     processor frequency. You can then use this symbol in your source code to
73
#     calculate timings. Do NOT tack on a 'UL' at the end, this will be done
74
#     automatically to create a 32-bit value in your source code.
75
F_CPU = 8000000
76

  
77
# Output format. (can be srec, ihex, binary)
78
FORMAT = ihex
79

  
80
# List C source files here. (C dependencies are automatically generated.)
81
SRC = $(wildcard *.c)
82

  
83
# List Assembler source files here.
84
#     Make them always end in a capital .S.  Files ending in a lowercase .s
85
#     will not be considered source files but generated files (assembler
86
#     output from the compiler), and will be deleted upon "make clean"!
87
#     Even though the DOS/Win* filesystem matches both .s and .S the same,
88
#     it will preserve the spelling of the filenames, and gcc itself does
89
#     care about how the name is spelled on its command-line.
90
ASRC =
91

  
92
# Optimization level, can be [0, 1, 2, 3, s].
93
#     0 = turn off optimization. s = optimize for size.
94
#     (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
95
OPT = s
96

  
97
# Debugging format.
98
#     Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs.
99
#     AVR Studio 4.10 requires dwarf-2.
100
#     AVR [Extended] COFF format requires stabs, plus an avr-objcopy run.
101
DEBUG =
102

  
103
# Compiler flag to set the C Standard level.
104
#     c89   = "ANSI" C
105
#     gnu89 = c89 plus GCC extensions
106
#     c99   = ISO C99 standard (not yet fully implemented)
107
#     gnu99 = c99 plus GCC extensions
108
CSTANDARD = -std=gnu99
109

  
110
# Place -D or -U options here
111
CDEFS += -DF_CPU=$(F_CPU)UL
112
CDEFS += -DFFP
113
# for wireless library
114
ifdef USE_WIRELESS
115
	CDEFS += -DROBOT
116
endif
117

  
118
# Place -I, -L options here
119
CINCS = -I$(COLONYROOT)/code/lib/include/libdragonfly
120
CINCS += -L$(COLONYROOT)/code/lib/bin
121
ifdef USE_WIRELESS
122
	CINCS += -I$(COLONYROOT)/code/lib/include/libwireless
123
endif
124

  
125
#---------------- Compiler Options ----------------
126
#  -g*:          generate debugging information
127
#  -O*:          optimization level
128
#  -f...:        tuning, see GCC manual and avr-libc documentation
129
#  -Wall...:     warning level
130
#  -Wa,...:      tell GCC to pass this to the assembler.
131
#    -adhlns...: create assembler listing
132
CFLAGS =
133
# CFLAGS = -g$(DEBUG)
134
CFLAGS += $(CDEFS) $(CINCS)
135
CFLAGS += -O$(OPT)
136
CFLAGS += -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums
137
CFLAGS += -Wall -Wstrict-prototypes
138
CFLAGS += -Wa,-adhlns=$(<:.c=.lst)
139
CFLAGS += $(CSTANDARD)
140

  
141
#---------------- Assembler Options ----------------
142
#  -Wa,...:   tell GCC to pass this to the assembler.
143
#  -ahlms:    create listing
144
#  -gstabs:   have the assembler create line number information; note that
145
#             for use in COFF files, additional information about filenames
146
#             and function names needs to be present in the assembler source
147
#             files -- see avr-libc docs [FIXME: not yet described there]
148
ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs
149

  
150

  
151
#---------------- Library Options ----------------
152
# Minimalistic printf version
153
PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min
154

  
155
# Floating point printf version (requires MATH_LIB = -lm below)
156
PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt
157

  
158
# If this is left blank, then it will use the Standard printf version.
159
#PRINTF_LIB =
160
#PRINTF_LIB = $(PRINTF_LIB_MIN)
161
PRINTF_LIB = $(PRINTF_LIB_FLOAT)
162

  
163

  
164
# Minimalistic scanf version
165
SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min
166

  
167
# Floating point + %[ scanf version (requires MATH_LIB = -lm below)
168
SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt
169

  
170
# If this is left blank, then it will use the Standard scanf version.
171
SCANF_LIB =
172
#SCANF_LIB = $(SCANF_LIB_MIN)
173
#SCANF_LIB = $(SCANF_LIB_FLOAT)
174

  
175
MATH_LIB = -lm
176

  
177
#---------------- External Memory Options ----------------
178

  
179
# 64 KB of external RAM, starting after internal RAM (ATmega128!),
180
# used for variables (.data/.bss) and heap (malloc()).
181
#EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff
182

  
183
# 64 KB of external RAM, starting after internal RAM (ATmega128!),
184
# only used for heap (malloc()).
185
#EXTMEMOPTS = -Wl,--defsym=__heap_start=0x801100,--defsym=__heap_end=0x80ffff
186

  
187
EXTMEMOPTS =
188

  
189
#---------------- Linker Options ----------------
190
#  -Wl,...:     tell GCC to pass this to linker.
191
#    -Map:      create map file
192
#    --cref:    add cross reference to  map file
193
LDFLAGS = -Wl,-Map=$(TARGET).map,--cref
194
LDFLAGS += $(EXTMEMOPTS)
195
LDFLAGS += $(PRINTF_LIB) $(SCANF_LIB) $(MATH_LIB)
196
ifdef USE_WIRELESS
197
	LDFLAGS += -lwireless
198
endif
199
LDFLAGS += -ldragonfly
200

  
201

  
202

  
203
#---------------- Programming Options (avrdude) ----------------
204

  
205
# Programming hardware: alf avr910 avrisp bascom bsd
206
# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500
207
#
208
# Type: avrdude -c ?
209
# to get a full listing.
210
#
211
AVRDUDE_PROGRAMMER = avrisp
212

  
213
# programmer connected to serial device
214

  
215
AVRDUDE_WRITE_FLASH = -b 57600 -U flash:w:$(TARGET).hex
216
#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep
217

  
218

  
219
# Uncomment the following if you want avrdude's erase cycle counter.
220
# Note that this counter needs to be initialized first using -Yn,
221
# see avrdude manual.
222
#AVRDUDE_ERASE_COUNTER = -y
223

  
224
# Uncomment the following if you do /not/ wish a verification to be
225
# performed after programming the device.
226
#AVRDUDE_NO_VERIFY = -V
227

  
228
# Increase verbosity level.  Please use this when submitting bug
229
# reports about avrdude. See <http://savannah.nongnu.org/projects/avrdude>
230
# to submit bug reports.
231
#AVRDUDE_VERBOSE = -v -v
232

  
233
AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER)
234
AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY)
235
AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE)
236
AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER)
237

  
238
#don't check for device signature
239
AVRDUDE_FLAGS += -F
240

  
241

  
242

  
243
#---------------- Debugging Options ----------------
244

  
245
# For simulavr only - target MCU frequency.
246
DEBUG_MFREQ = $(F_CPU)
247

  
248
# Set the DEBUG_UI to either gdb or insight.
249
# DEBUG_UI = gdb
250
DEBUG_UI = insight
251

  
252
# Set the debugging back-end to either avarice, simulavr.
253
DEBUG_BACKEND = avarice
254
#DEBUG_BACKEND = simulavr
255

  
256
# GDB Init Filename.
257
GDBINIT_FILE = __avr_gdbinit
258

  
259
# When using avarice settings for the JTAG
260
JTAG_DEV = /dev/com1
261

  
262
# Debugging port used to communicate between GDB / avarice / simulavr.
263
DEBUG_PORT = 4242
264

  
265
# Debugging host used to communicate between GDB / avarice / simulavr, normally
266
#     just set to localhost unless doing some sort of crazy debugging when
267
#     avarice is running on a different computer.
268
DEBUG_HOST = localhost
269

  
270

  
271

  
272
#============================================================================
273

  
274

  
275
# Define programs and commands.
276
SHELL = sh
277
CC = avr-gcc
278
OBJCOPY = avr-objcopy
279
OBJDUMP = avr-objdump
280
SIZE = avr-size
281
NM = avr-nm
282
AVRDUDE = avrdude
283
REMOVE = rm -f
284
REMOVEDIR = rm -rf
285
COPY = cp
286
WINSHELL = cmd
287

  
288

  
289
# Define Messages
290
# English
291
MSG_ERRORS_NONE = Errors: none
292
MSG_BEGIN = -------- begin --------
293
MSG_END = --------  end  --------
294
MSG_SIZE_BEFORE = Size before:
295
MSG_SIZE_AFTER = Size after:
296
MSG_COFF = Converting to AVR COFF:
297
MSG_EXTENDED_COFF = Converting to AVR Extended COFF:
298
MSG_FLASH = Creating load file for Flash:
299
MSG_EEPROM = Creating load file for EEPROM:
300
MSG_EXTENDED_LISTING = Creating Extended Listing:
301
MSG_SYMBOL_TABLE = Creating Symbol Table:
302
MSG_LINKING = Linking:
303
MSG_COMPILING = Compiling:
304
MSG_ASSEMBLING = Assembling:
305
MSG_CLEANING = Cleaning project:
306

  
307

  
308

  
309

  
310
# Define all object files.
311
OBJ = $(SRC:.c=.o) $(ASRC:.S=.o)
312

  
313
# Define all listing files.
314
LST = $(SRC:.c=.lst) $(ASRC:.S=.lst)
315

  
316

  
317
# Compiler flags to generate dependency files.
318
GENDEPFLAGS = -MD -MP -MF .dep/$(@F).d
319

  
320

  
321
# Combine all necessary flags and optional flags.
322
# Add target processor to flags.
323
ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS)
324
ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS)
325

  
326

  
327

  
328

  
329

  
330
# Default target.
331
all: begin gccversion sizebefore build sizeafter end
332

  
333
build: elf hex eep lss sym
334

  
335
elf: $(TARGET).elf
336
hex: $(TARGET).hex
337
eep: $(TARGET).eep
338
lss: $(TARGET).lss
339
sym: $(TARGET).sym
340

  
341

  
342

  
343
# Eye candy.
344
# AVR Studio 3.x does not check make's exit code but relies on
345
# the following magic strings to be generated by the compile job.
346
begin:
347
	@echo
348
	@echo $(MSG_BEGIN)
349

  
350
end:
351
	@echo $(MSG_END)
352
	@echo
353

  
354

  
355
# Display size of file.
356
HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex
357
ELFSIZE = $(SIZE) -A $(TARGET).elf
358
AVRMEM = avr-mem.sh $(TARGET).elf $(MCU)
359

  
360
sizebefore:
361
	@if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); \
362
	$(AVRMEM) 2>/dev/null; echo; fi
363

  
364
sizeafter:
365
	@if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); \
366
	$(AVRMEM) 2>/dev/null; echo; fi
367

  
368

  
369

  
370
# Display compiler version information.
371
gccversion :
372
	@$(CC) --version
373

  
374

  
375

  
376
# Program the device.
377
program: $(TARGET).hex $(TARGET).eep
378
	$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)
379

  
380

  
381
# Generate avr-gdb config/init file which does the following:
382
#     define the reset signal, load the target file, connect to target, and set
383
#     a breakpoint at main().
384
gdb-config:
385
	@$(REMOVE) $(GDBINIT_FILE)
386
	@echo define reset >> $(GDBINIT_FILE)
387
	@echo SIGNAL SIGHUP >> $(GDBINIT_FILE)
388
	@echo end >> $(GDBINIT_FILE)
389
	@echo file $(TARGET).elf >> $(GDBINIT_FILE)
390
	@echo target remote $(DEBUG_HOST):$(DEBUG_PORT)  >> $(GDBINIT_FILE)
391
ifeq ($(DEBUG_BACKEND),simulavr)
392
	@echo load  >> $(GDBINIT_FILE)
393
endif
394
	@echo break main >> $(GDBINIT_FILE)
395

  
396
debug: gdb-config $(TARGET).elf
397
ifeq ($(DEBUG_BACKEND), avarice)
398
	@echo Starting AVaRICE - Press enter when "waiting to connect" message displays.
399
	@$(WINSHELL) /c start avarice --jtag $(JTAG_DEV) --erase --program --file \
400
	$(TARGET).elf $(DEBUG_HOST):$(DEBUG_PORT)
401
	@$(WINSHELL) /c pause
402

  
403
else
404
	@$(WINSHELL) /c start simulavr --gdbserver --device $(MCU) --clock-freq \
405
	$(DEBUG_MFREQ) --port $(DEBUG_PORT)
406
endif
407
	@$(WINSHELL) /c start avr-$(DEBUG_UI) --command=$(GDBINIT_FILE)
408

  
409

  
410

  
411

  
412
# Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB.
413
COFFCONVERT=$(OBJCOPY) --debugging \
414
--change-section-address .data-0x800000 \
415
--change-section-address .bss-0x800000 \
416
--change-section-address .noinit-0x800000 \
417
--change-section-address .eeprom-0x810000
418

  
419

  
420
coff: $(TARGET).elf
421
	@echo
422
	@echo $(MSG_COFF) $(TARGET).cof
423
	$(COFFCONVERT) -O coff-avr $< $(TARGET).cof
424

  
425

  
426
extcoff: $(TARGET).elf
427
	@echo
428
	@echo $(MSG_EXTENDED_COFF) $(TARGET).cof
429
	$(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof
430

  
431

  
432

  
433
# Create final output files (.hex, .eep) from ELF output file.
434
%.hex: %.elf
435
	@echo
436
	@echo $(MSG_FLASH) $@
437
	$(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@
438

  
439
%.eep: %.elf
440
	@echo
441
	@echo $(MSG_EEPROM) $@
442
	-$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \
443
	--change-section-lma .eeprom=0 -O $(FORMAT) $< $@
444

  
445
# Create extended listing file from ELF output file.
446
%.lss: %.elf
447
	@echo
448
	@echo $(MSG_EXTENDED_LISTING) $@
449
	$(OBJDUMP) -h -S $< > $@
450

  
451
# Create a symbol table from ELF output file.
452
%.sym: %.elf
453
	@echo
454
	@echo $(MSG_SYMBOL_TABLE) $@
455
	$(NM) -n $< > $@
456

  
457

  
458

  
459
# Link: create ELF output file from object files.
460
.SECONDARY : $(TARGET).elf
461
.PRECIOUS : $(OBJ)
462
%.elf: $(OBJ)
463
	@echo
464
	@echo $(MSG_LINKING) $@
465
	$(CC) $(ALL_CFLAGS) $^ --output $@ $(LDFLAGS)
466

  
467

  
468
# Compile: create object files from C source files.
469
%.o : %.c
470
	@echo
471
	@echo $(MSG_COMPILING) $<
472
	$(CC) -c $(ALL_CFLAGS) $< -o $@
473

  
474

  
475
# Compile: create assembler files from C source files.
476
%.s : %.c
477
	$(CC) -S $(ALL_CFLAGS) $< -o $@
478

  
479

  
480
# Assemble: create object files from assembler source files.
481
%.o : %.S
482
	@echo
483
	@echo $(MSG_ASSEMBLING) $<
484
	$(CC) -c $(ALL_ASFLAGS) $< -o $@
485

  
486
# Create preprocessed source for use in sending a bug report.
487
%.i : %.c
488
	$(CC) -E -mmcu=$(MCU) -I. $(CFLAGS) $< -o $@
489

  
490

  
491
# Target: clean project.
492
clean: begin clean_list end
493

  
494
clean_list :
495
	@echo
496
	@echo $(MSG_CLEANING)
497
	$(REMOVE) $(TARGET).hex
498
	$(REMOVE) $(TARGET).eep
499
	$(REMOVE) $(TARGET).cof
500
	$(REMOVE) $(TARGET).elf
501
	$(REMOVE) $(TARGET).map
502
	$(REMOVE) $(TARGET).sym
503
	$(REMOVE) $(TARGET).lss
504
	$(REMOVE) $(OBJ)
505
	$(REMOVE) $(LST)
506
	$(REMOVE) $(SRC:.c=.s)
507
	$(REMOVE) $(SRC:.c=.d)
508
	$(REMOVEDIR) .dep
509

  
510

  
511

  
512
# Include the dependency files.
513
-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*)
514

  
515

  
516
# Listing of phony targets.
517
.PHONY : all begin finish end sizebefore sizeafter gccversion \
518
build elf hex eep lss sym coff extcoff \
519
clean clean_list program debug gdb-config
520

  
branches/analog/code/projects/libwireless/lib/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

  
35
#include <wl_token_ring.h>
36

  
37
#include <stdlib.h>
38
#include <stdio.h>
39

  
40
#include <wl_defs.h>
41
#include <wireless.h>
42
#include <sensor_matrix.h>
43

  
44
#ifdef ROBOT
45
#ifndef FIREFLY
46
#include <bom.h>
47
#endif
48
#include <time.h>
49
#endif
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
53
#define DEFAULT_SENSOR_MATRIX_SIZE 20
54

  
55
/*Ring States*/
56

  
57
#define NONMEMBER 0
58
#define MEMBER 1
59
#define JOINING 2
60
#define ACCEPTED 3
61
#define LEAVING 4
62

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

  
67
/*Function Prototypes*/
68

  
69
/*Wireless Library Prototypes*/
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);
74

  
75
/*Helper Functions*/
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);
79

  
80
/*Packet Handling Routines*/
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);
86

  
87
/*Global Variables*/
88

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

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

  
99
//the counter for when we assume a robot is dead
100
static int deathDelay = -1;
101
//the counter for joining, before we form our own token ring
102
static int joinDelay = -1;
103

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

  
107
// the amount of time a robot has had its BOM on for
108
static int bom_on_count = 0;
109

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

  
115
#ifdef ROBOT
116
#ifndef FIREFLY
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;
120
#else
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;
124
#endif
125
#else
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;
129
#endif
130

  
131
static PacketGroupHandler wl_token_ring_handler =
132
	{WL_TOKEN_RING_GROUP, wl_token_ring_timeout_handler,
133
		wl_token_ring_response_handler, wl_token_ring_receive_handler,
134
		wl_token_ring_cleanup};
135

  
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
/**
167
 * Initialize the token ring packet group and register it with the
168
 * wireless library. The robot will not join a token ring.
169
 **/
170
int wl_token_ring_register()
171
{
172
	if (wl_get_xbee_id() > 0xFF)
173
	{
174
		//Note: if this becomes an issue (unlikely), we could limit sensor information
175
		//to half a byte and use 12 bits for the id
176
		WL_DEBUG_PRINT("XBee ID must be single byte for token ring, is ");
177
		WL_DEBUG_PRINT_INT(wl_get_xbee_id());
178
		WL_DEBUG_PRINT(".\r\n");
179
		return -1;
180
	}
181

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

  
186
	wl_register_packet_group(&wl_token_ring_handler);
187

  
188
	return 0;
189
}
190

  
191
/**
192
 * Removes the packet group from the wireless library.
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff