root / trunk / code / projects / hunter_prey / abenico / main.c @ 1464
History | View | Annotate | Download (3.92 KB)
1 | 1464 | alevkoy | #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 ACK_PAUSE 5000 |
||
9 | |||
10 | #define ROBOT_HUNTER 0 |
||
11 | #define ROBOT_PREY 1 |
||
12 | #define ROBOT_TAGGED 2 |
||
13 | #define ROBOT_IDLE 3 |
||
14 | |||
15 | void packet_receive(unsigned char* packet, int length); |
||
16 | |||
17 | volatile uint8_t robotState; // current state |
||
18 | volatile uint8_t tagger; // id of tagging robot |
||
19 | uint8_t id; // my robot's id
|
||
20 | |||
21 | void test_bom(void) { |
||
22 | int i;
|
||
23 | int val;
|
||
24 | |||
25 | while(1) { |
||
26 | bom_refresh(BOM_ALL); |
||
27 | |||
28 | for(i=0; i<16; i++) { |
||
29 | val = bom_get(i); |
||
30 | usb_puti(val); |
||
31 | usb_putc(' ');
|
||
32 | } |
||
33 | |||
34 | usb_puts("| ");
|
||
35 | |||
36 | usb_puti(range_read_distance (IR1)); |
||
37 | usb_putc(' ');
|
||
38 | usb_puti(range_read_distance (IR2)); |
||
39 | usb_putc(' ');
|
||
40 | usb_puti(range_read_distance (IR3)); |
||
41 | usb_putc(' ');
|
||
42 | usb_puti(range_read_distance (IR4)); |
||
43 | usb_putc(' ');
|
||
44 | usb_puti(range_read_distance (IR5)); |
||
45 | |||
46 | usb_putc('\n');
|
||
47 | delay_ms(500);
|
||
48 | } |
||
49 | |||
50 | } |
||
51 | |||
52 | void chase(uint8_t angle) {
|
||
53 | |||
54 | int omega = angle - 4; |
||
55 | |||
56 | if(angle > 15) |
||
57 | return;
|
||
58 | |||
59 | if(omega > 8) |
||
60 | omega = omega - 16;
|
||
61 | |||
62 | omega *= 255/8; |
||
63 | |||
64 | /* usb_puti(omega); */
|
||
65 | /* usb_putc('\n'); */
|
||
66 | |||
67 | move(140, omega);
|
||
68 | |||
69 | } |
||
70 | |||
71 | int main(void) { |
||
72 | |||
73 | int frontRange; // reading of front rangefinder |
||
74 | uint8_t angle; // result of bom_get_max()
|
||
75 | unsigned char *packet; // packet to send |
||
76 | int length; // length of received packet |
||
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_default(); |
||
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 | usb_puti(CHAN); |
||
102 | |||
103 | usb_puts("orb set\n");
|
||
104 | |||
105 | // initially in hunter state
|
||
106 | robotState = ROBOT_HUNTER; |
||
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 | orb_set_color(GREEN); |
||
133 | run_around_FSM(); |
||
134 | } |
||
135 | |||
136 | else if (robotState == ROBOT_IDLE) { |
||
137 | orb_set_color(BLUE); |
||
138 | move(0,0); |
||
139 | delay_ms(ACK_PAUSE); |
||
140 | robotState = ROBOT_HUNTER; |
||
141 | } |
||
142 | |||
143 | else { //ROBOT_HUNTER |
||
144 | orb_set_color(RED); |
||
145 | bom_refresh(BOM_ALL); |
||
146 | delay_ms(100);
|
||
147 | |||
148 | frontRange = range_read_distance (IR2); |
||
149 | |||
150 | //BUG: when get_max_bom is called, the rangefinder seems to stop
|
||
151 | //working
|
||
152 | angle = bom_get_max(); |
||
153 | chase(angle); |
||
154 | |||
155 | usb_puti(frontRange); |
||
156 | usb_putc('\n');
|
||
157 | |||
158 | if ((hunter_prey_tagged(angle, frontRange) && rtc_get() >= 1) || button1_read()) { |
||
159 | usb_puts("TAG!\n");
|
||
160 | packet[0] = HUNTER_PREY_ACTION_TAG;
|
||
161 | packet[1] = id;
|
||
162 | wl_basic_send_global_packet (42, &packet, 2); |
||
163 | rtc_reset(); |
||
164 | } |
||
165 | } |
||
166 | |||
167 | packet = wl_basic_do_default(&length); |
||
168 | if (packet)
|
||
169 | packet_receive(packet, length); |
||
170 | } |
||
171 | |||
172 | return 0; |
||
173 | } |
||
174 | |||
175 | void packet_receive(unsigned char* packet, int length) |
||
176 | { |
||
177 | |||
178 | if(length>=2){ |
||
179 | if(packet[0] == HUNTER_PREY_ACTION_TAG) { |
||
180 | if(robotState == ROBOT_PREY) {
|
||
181 | robotState = ROBOT_TAGGED; |
||
182 | bom_off(); |
||
183 | tagger = packet[1];
|
||
184 | } |
||
185 | else {
|
||
186 | usb_puts("tagged, but I'm not it!\n");
|
||
187 | } |
||
188 | } |
||
189 | else if(packet[0] == HUNTER_PREY_ACTION_ACK && robotState == ROBOT_HUNTER) { |
||
190 | robotState = ROBOT_IDLE; |
||
191 | } |
||
192 | usb_puts("got '");
|
||
193 | usb_putc(packet[0]);
|
||
194 | usb_puti(packet[1]);
|
||
195 | usb_puts("'\n");
|
||
196 | } |
||
197 | else {
|
||
198 | usb_puts(" length=");
|
||
199 | usb_puti(length); |
||
200 | usb_putc('\n');
|
||
201 | } |
||
202 | } |
||
203 | |||
204 | #define HP_BUF_IDX_ACTION 0 |