Revision 1447
fixed testbench, now it works!
also fixed hunter prey reference to allow packet type of 42, but there is still a bug in the reference WRT the latest spec because it waits indefinitely for an ACK
trunk/code/projects/hunter_prey/testbench/main.c | ||
---|---|---|
24 | 24 |
#include "../../libwireless/lib/wl_basic.h" |
25 | 25 |
#include "../../libwireless/lib/wireless.h" |
26 | 26 |
#include "hunter_prey.h" |
27 |
#include <time.h> |
|
27 | 28 |
|
28 | 29 |
#define CHANNEL 0xF // channel for wireless communication |
29 | 30 |
#define TYPE 42 // packet type for wireless communication |
... | ... | |
34 | 35 |
char send_buffer[2]; // holds data to send |
35 | 36 |
int data_length; // length of data received |
36 | 37 |
unsigned char *packet_data; // data received |
38 |
int ret; |
|
37 | 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"); |
|
38 | 46 |
// set up wireless |
39 | 47 |
wl_basic_init_default(); |
40 | 48 |
wl_set_channel(CHANNEL); |
41 |
wl_set_com_port("/dev/ttyUSB0"); |
|
42 | 49 |
|
43 | 50 |
printf("Testing communications\n\n"); |
44 | 51 |
|
... | ... | |
48 | 55 |
// Wait until we receive a packet |
49 | 56 |
while (!(packet_data = wl_basic_do_default(&data_length))); |
50 | 57 |
|
58 |
printf("got packet, validating and sending ack..."); |
|
59 |
fflush(stdout); |
|
60 |
|
|
51 | 61 |
if (data_length > 2) |
52 | 62 |
{ |
53 | 63 |
printf("Excessive TAG packet length... "); |
... | ... | |
59 | 69 |
// send back an ACK |
60 | 70 |
send_buffer[0] = HUNTER_PREY_ACTION_ACK; |
61 | 71 |
send_buffer[1] = packet_data[1]; |
72 |
printf("sending ack intended for robot %d\n", packet_data[1]); |
|
73 |
fflush(stdout); |
|
62 | 74 |
wl_basic_send_global_packet(TYPE, send_buffer, 2); |
63 | 75 |
|
64 | 76 |
printf("PASSED\n"); |
... | ... | |
66 | 78 |
else |
67 | 79 |
printf("FAILED\n"); |
68 | 80 |
|
81 |
|
|
82 |
sleep(10); |
|
83 |
|
|
69 | 84 |
// Send a TAG, receive an ACK |
70 | 85 |
printf("Send TAG, wait for ACK... "); |
71 | 86 |
fflush(stdout); |
... | ... | |
76 | 91 |
wl_basic_send_global_packet(TYPE, send_buffer, 2); |
77 | 92 |
|
78 | 93 |
// Wait for an ACK |
79 |
usleep(800000); // wait for 800 ms before sending 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; |
|
80 | 102 |
packet_data = wl_basic_do_default(&data_length); |
81 | 103 |
|
82 | 104 |
// Check ACK for presence and correctness |
83 | 105 |
if (!packet_data || data_length < 2 || |
84 | 106 |
packet_data[0] != HUNTER_PREY_ACTION_ACK || |
85 | 107 |
packet_data[1] != ROBOTID) |
86 |
printf("FAILED\n");
|
|
108 |
printf("FAILED, packet=%p, len=%d\n", packet_data, data_length);
|
|
87 | 109 |
else |
88 | 110 |
printf("PASSED\n"); |
89 | 111 |
|
... | ... | |
95 | 117 |
|
96 | 118 |
if (data_length >= 2 && packet_data[0] == HUNTER_PREY_ACTION_TAG) |
97 | 119 |
{ |
120 |
printf("got TAG, waiting..."); |
|
121 |
fflush(stdout); |
|
98 | 122 |
// wait before sending ACK back |
99 |
usleep(900000); |
|
123 |
//usleep(900000); |
|
124 |
delay8.tv_sec = 0; |
|
125 |
ret = nanosleep(&delay8, &rem); // wait for 800 ms before sending ACK |
|
100 | 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 |
|
|
101 | 134 |
if (wl_basic_do_default(&data_length)) |
102 |
printf("FAILED\n"); |
|
135 |
printf("FAILED, got another TAG too soon\n");
|
|
103 | 136 |
else |
104 | 137 |
{ |
105 | 138 |
// send packet |
... | ... | |
114 | 147 |
else |
115 | 148 |
printf("FAILED\n"); |
116 | 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 |
|
|
117 | 170 |
// Receive a TAG, never send an ACK |
118 | 171 |
printf("Receive TAG, never send ACK... "); |
119 | 172 |
fflush(stdout); |
... | ... | |
122 | 175 |
|
123 | 176 |
if (data_length >= 2 && packet_data[0] == HUNTER_PREY_ACTION_TAG) |
124 | 177 |
{ |
178 |
|
|
179 |
printf("got TAG, monitoring for 5 seconds\n"); |
|
180 |
fflush(stdout); |
|
125 | 181 |
usleep(5000000); // wait 5 seconds to see if they TAG again |
126 | 182 |
|
127 | 183 |
if (wl_basic_do_default(&data_length)) |
128 |
printf("FAILED\n"); |
|
184 |
printf("FAILED, robot is spamming\n");
|
|
129 | 185 |
else |
130 | 186 |
printf("PASSED\n"); |
131 | 187 |
} |
trunk/code/projects/hunter_prey/main.c | ||
---|---|---|
116 | 116 |
|
117 | 117 |
if(robotState==ROBOT_TAGGED) { |
118 | 118 |
usb_puts("tagged, waiting to send ACK\n"); |
119 |
delay_ms(TAG_PAUSE); |
|
119 |
//delay_ms(TAG_PAUSE);
|
|
120 | 120 |
move(0,0); |
121 | 121 |
usb_puts("sending ACK to "); |
122 | 122 |
usb_puti(tagger); |
123 | 123 |
usb_putc('\n'); |
124 | 124 |
packet[0] = HUNTER_PREY_ACTION_ACK; |
125 | 125 |
packet[1] = tagger; |
126 |
wl_basic_send_global_packet (0, &packet, 2);
|
|
126 |
wl_basic_send_global_packet (42, &packet, 2);
|
|
127 | 127 |
usb_puts("sent\n"); |
128 | 128 |
robotState = ROBOT_HUNTER; //no longer prey |
129 | 129 |
} |
... | ... | |
184 | 184 |
|
185 | 185 |
|
186 | 186 |
|
187 |
if(hunter_prey_tagged(angle, frontRange)) { |
|
187 |
if(hunter_prey_tagged(angle, frontRange) || button1_read()) {
|
|
188 | 188 |
orb_set_color(YELLOW); |
189 | 189 |
color1 = YELLOW; |
190 | 190 |
color2 = YELLOW; |
... | ... | |
192 | 192 |
tagAck = 0; |
193 | 193 |
packet[0] = HUNTER_PREY_ACTION_TAG; |
194 | 194 |
packet[1] = id; |
195 |
wl_basic_send_global_packet (0, &packet, 2);
|
|
195 |
wl_basic_send_global_packet (42, &packet, 2);
|
|
196 | 196 |
move(0,0); |
197 | 197 |
while(!tagAck) |
198 | 198 |
wl_do(); |
... | ... | |
222 | 222 |
void packet_receive(char type, int source, unsigned char* packet, int length) |
223 | 223 |
{ |
224 | 224 |
|
225 |
if(type==0 && length>=2){
|
|
225 |
if(type==42 && length>=2){
|
|
226 | 226 |
if(packet[0] == HUNTER_PREY_ACTION_TAG) { |
227 | 227 |
if(robotState == ROBOT_PREY) { |
228 | 228 |
robotState = ROBOT_TAGGED; |
trunk/code/projects/hunter_prey/smart_run_around_fsm.c | ||
---|---|---|
87 | 87 |
{ |
88 | 88 |
case SRA_FORWARD: |
89 | 89 |
orb_set_color(GREEN); |
90 |
move(FULL_SPD, 0); // drive straight forward
|
|
90 |
move(220, 0); // drive straight forward
|
|
91 | 91 |
break; |
92 | 92 |
case SRA_LEFT: |
93 | 93 |
orbs_set_color(GREEN, YELLOW); // green on left side |
Also available in: Unified diff