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