root / trunk / code / projects / hunter_prey / main.c @ 1408
History | View | Annotate | Download (3.51 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wireless.h> |
3 |
#include <wl_basic.h> |
4 |
#include "smart_run_around_fsm.h" |
5 |
|
6 |
#define CHAN 0xF |
7 |
#define TAG_TIME 10 |
8 |
#define TAG_RANGE 150 |
9 |
#define TAG_PAUSE 2000 |
10 |
|
11 |
#define ROBOT_HUNTER 0 |
12 |
#define ROBOT_PREY 1 |
13 |
#define ROBOT_TAGGED 2 |
14 |
|
15 |
void packet_receive(char type, int source, unsigned char* packet, int length); |
16 |
|
17 |
volatile uint8_t robotState = ROBOT_HUNTER;
|
18 |
volatile uint8_t color;
|
19 |
volatile uint8_t tagAck;
|
20 |
volatile uint8_t tagger;
|
21 |
uint8_t id; |
22 |
|
23 |
int main(void) { |
24 |
|
25 |
int frontRange;
|
26 |
uint8_t angle; |
27 |
uint8_t i; |
28 |
int onTarget = 0; |
29 |
char packet[2]; |
30 |
|
31 |
//intialize the robot and wireless
|
32 |
dragonfly_init(ALL_ON); |
33 |
|
34 |
id = get_robotid(); |
35 |
packet[1] = id;
|
36 |
usb_puts("hello, I am robot \n");
|
37 |
usb_puti(id); |
38 |
usb_putc('\n');
|
39 |
|
40 |
usb_puts("wireless.\n");
|
41 |
|
42 |
wl_basic_init(&packet_receive); |
43 |
|
44 |
usb_puts("run around.\n");
|
45 |
|
46 |
run_around_init(); |
47 |
|
48 |
// bom_init(BOM15);
|
49 |
|
50 |
usb_puts("channel\n");
|
51 |
|
52 |
//use a specific channel to avoid interfering with other tasks
|
53 |
wl_set_channel(CHAN); |
54 |
|
55 |
usb_puts("orb set\n");
|
56 |
|
57 |
orb_set_color(BLUE); |
58 |
color = BLUE; |
59 |
|
60 |
//if button 2 is held, you are prey
|
61 |
if(button2_read()) {
|
62 |
robotState = ROBOT_PREY; |
63 |
} |
64 |
|
65 |
|
66 |
while(1) { |
67 |
|
68 |
if(robotState==ROBOT_TAGGED) {
|
69 |
usb_puts("tagged, waiting to send ACK\n");
|
70 |
move(0,0); |
71 |
delay_ms(TAG_PAUSE); |
72 |
usb_puts("sending ACK to ");
|
73 |
usb_puti(tagger); |
74 |
usb_putc('\n');
|
75 |
packet[0] = 'A'; |
76 |
packet[1] = tagger;
|
77 |
wl_basic_send_global_packet (0, packet, 2); |
78 |
usb_puts("sent\n");
|
79 |
move(0,0); |
80 |
robotState = ROBOT_HUNTER; //no longer prey
|
81 |
} |
82 |
|
83 |
else if(robotState == ROBOT_PREY) { |
84 |
run_around_FSM(); |
85 |
} |
86 |
|
87 |
else { //ROBOT_HUNTER |
88 |
|
89 |
// bom_refresh(BOM_ALL);
|
90 |
|
91 |
frontRange = range_read_distance (IR2); |
92 |
|
93 |
/* usb_puti(frontRange); */
|
94 |
/* usb_putc('\n'); */
|
95 |
|
96 |
//BUG: when get_max_bom is called, the rangefinder seems to stop
|
97 |
//working
|
98 |
//angle = get_max_bom();
|
99 |
angle = 4;
|
100 |
|
101 |
if(angle == 4 && frontRange > 0 && frontRange < TAG_RANGE) { |
102 |
if(onTarget == 0) { |
103 |
onTarget = TAG_TIME; |
104 |
if(color != GREEN) {
|
105 |
color = GREEN; |
106 |
orb_set_color(GREEN); |
107 |
} |
108 |
usb_puts("On target!\n");
|
109 |
} |
110 |
else {
|
111 |
if(--onTarget <= 0) { |
112 |
orb_set_color(YELLOW); |
113 |
color = YELLOW; |
114 |
usb_puts("TAG!\n");
|
115 |
tagAck = 0;
|
116 |
packet[0] = 'T'; |
117 |
packet[1] = id;
|
118 |
wl_basic_send_global_packet (0, packet, 2); |
119 |
move(0,0); |
120 |
while(!tagAck)
|
121 |
wl_do(); |
122 |
|
123 |
if(tagAck == id) { //if the ack was for us |
124 |
usb_puts("ACK!\n");
|
125 |
// delay_ms(TAG_PAUSE);
|
126 |
robotState = ROBOT_PREY; |
127 |
} |
128 |
else {
|
129 |
usb_puts("ACK failed!\n");
|
130 |
} |
131 |
} |
132 |
} |
133 |
} |
134 |
else{
|
135 |
if(color != BLUE) {
|
136 |
color = BLUE; |
137 |
orb_set_color(BLUE); |
138 |
} |
139 |
|
140 |
//don't reset onTarget because the robot got too close
|
141 |
if(frontRange > 0) |
142 |
onTarget = 0;
|
143 |
} |
144 |
|
145 |
} |
146 |
|
147 |
wl_do(); |
148 |
} |
149 |
|
150 |
return 0; |
151 |
} |
152 |
|
153 |
void packet_receive(char type, int source, unsigned char* packet, int length) |
154 |
{ |
155 |
if(type==0 && length>=2){ |
156 |
if(packet[0] == 'T') { |
157 |
if(robotState == ROBOT_PREY) {
|
158 |
robotState = ROBOT_TAGGED; |
159 |
color = BLUE; |
160 |
orb_set_color(BLUE); |
161 |
tagger = packet[1];
|
162 |
} |
163 |
else {
|
164 |
usb_puts("tagged, but I'm not it!\n");
|
165 |
} |
166 |
} |
167 |
else if(packet[0] == 'A' && robotState == ROBOT_HUNTER) { |
168 |
tagAck = packet[1];
|
169 |
} |
170 |
else {
|
171 |
usb_puts("got '");
|
172 |
usb_putc(packet[0]);
|
173 |
usb_puti(packet[1]);
|
174 |
usb_puts("'\n");
|
175 |
} |
176 |
} |
177 |
else {
|
178 |
usb_puts("got unkown packet! type=");
|
179 |
usb_puti(type); |
180 |
usb_puts(" length=");
|
181 |
usb_puti(length); |
182 |
usb_putc('\n');
|
183 |
} |
184 |
} |
185 |
|