Revision 1447 trunk/code/projects/hunter_prey/testbench/main.c
| main.c (revision 1447) | ||
|---|---|---|
| 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 |
} |
Also available in: Unified diff