root / branches / colonetmk2 / code / projects / swarm / test.cpp @ 1456
History | View | Annotate | Download (3.07 KB)
1 | 1456 | rcahoon | #include <colonet_wireless.h> |
---|---|---|---|
2 | #include <wireless.h> |
||
3 | #include <stdio.h> |
||
4 | #include <string.h> |
||
5 | #include <stdlib.h> |
||
6 | #include <signal.h> |
||
7 | #include <unistd.h> |
||
8 | |||
9 | #include <pthread.h> |
||
10 | |||
11 | #define SWARM_PACKET_GROUP_ID 0x8 |
||
12 | |||
13 | static PacketGroupHandler colonet_pgh;
|
||
14 | static void colonet_handle_receive(char type, int wl_source, unsigned char* packet, int length); |
||
15 | |||
16 | void cleanup(int a) |
||
17 | { |
||
18 | a = a; |
||
19 | |||
20 | wl_terminate(); |
||
21 | printf("todo - close wl port\n");
|
||
22 | |||
23 | exit(0);
|
||
24 | } |
||
25 | |||
26 | int main(int argc, char* argv[]) |
||
27 | { |
||
28 | struct sigaction act;
|
||
29 | memset(&act, 0, sizeof(struct sigaction)); |
||
30 | act.sa_handler = cleanup; |
||
31 | |||
32 | if (argc < 2) return -1; |
||
33 | |||
34 | //register a signal handler to be called when we get a SIGINT signal (killing the server by cntrl+c would generate this)
|
||
35 | sigaction(SIGINT, &act, NULL);
|
||
36 | |||
37 | char *wireless_port = argv[1]; |
||
38 | |||
39 | //initialize the wireless library giving it the port and the log file name
|
||
40 | if (colonet_wl_init(wireless_port) != 0) { |
||
41 | printf("colonet_wl_init failed.\n");
|
||
42 | return -1; |
||
43 | } |
||
44 | |||
45 | colonet_pgh.groupCode = SWARM_PACKET_GROUP_ID; |
||
46 | colonet_pgh.timeout_handler = NULL;
|
||
47 | colonet_pgh.handle_response = NULL;
|
||
48 | colonet_pgh.handle_receive = colonet_handle_receive; |
||
49 | colonet_pgh.unregister = NULL;
|
||
50 | wl_register_packet_group(&colonet_pgh); |
||
51 | |||
52 | //call the function that will start the thread that will listen for wireless messages
|
||
53 | if (colonet_wl_run_listener_thread()) {
|
||
54 | printf("colonet_wl_run_listener_thread failed.\n");
|
||
55 | return -1; |
||
56 | } |
||
57 | |||
58 | unsigned char i, j; |
||
59 | int numRobots = 1; |
||
60 | //int* robots = (int *)malloc(0);
|
||
61 | int robots[] = {13}; |
||
62 | |||
63 | for(i=0; ; i++) |
||
64 | { |
||
65 | if (i==0) |
||
66 | { |
||
67 | //numRobots = colonet_get_num_robots();
|
||
68 | //robots = (int *)realloc(robots, sizeof(int) * numRobots);
|
||
69 | //colonet_get_xbee_ids(robots);
|
||
70 | } |
||
71 | |||
72 | /*for (j=0; j < numRobots; j++)
|
||
73 | {
|
||
74 | usleep(1000);
|
||
75 | |||
76 | printf("\rSending %d to %d ", i, robots[j]);
|
||
77 | |||
78 | char packet[] = { i, i, i, i };
|
||
79 | wl_send_robot_to_robot_packet(SWARM_PACKET_GROUP_ID, 0, packet, sizeof(char) * 4, robots[j], 0);
|
||
80 | }*/
|
||
81 | |||
82 | pthread_yield(); |
||
83 | } |
||
84 | |||
85 | //colonet_wl_join();
|
||
86 | printf("Exiting normally\n");
|
||
87 | |||
88 | return 0; |
||
89 | } |
||
90 | |||
91 | unsigned char count[] = { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; |
||
92 | |||
93 | static void colonet_handle_receive(char type, int wl_source, unsigned char* packet, int length) |
||
94 | { |
||
95 | type = type; |
||
96 | |||
97 | if (length != 17) |
||
98 | { |
||
99 | printf("Bad packet length from %d\n", wl_source);
|
||
100 | return;
|
||
101 | } |
||
102 | |||
103 | for (int i = 0; i < length; i++) |
||
104 | { |
||
105 | /*if (packet[i] != i)
|
||
106 | {
|
||
107 | printf("Corrupted packet from %d\n", wl_source);
|
||
108 | break;
|
||
109 | }*/
|
||
110 | printf("%d ", packet[i]);
|
||
111 | } |
||
112 | printf("\n");
|
||
113 | |||
114 | if (packet[16]!=++count[wl_source]) |
||
115 | printf("Bad packet number from %d (%d, should be %d)\n", wl_source, packet[16], count[wl_source]); |
||
116 | |||
117 | if (packet[16] == 0) |
||
118 | { |
||
119 | printf("Packet 0 received from %d\n", wl_source);
|
||
120 | } |
||
121 | |||
122 | count[wl_source] = packet[16];
|
||
123 | |||
124 | fflush(stdout); |
||
125 | } |