root / trunk / code / projects / libwireless / test / test.c @ 1860
History | View | Annotate | Download (2.76 KB)
1 | 897 | cmar | #include <stdlib.h> |
---|---|---|---|
2 | #include <stdio.h> |
||
3 | #include <curses.h> |
||
4 | 17 | bcoltin | #include "../lib/wireless.h" |
5 | |||
6 | 897 | cmar | #define WL_CNTL_GROUP 4 |
7 | 17 | bcoltin | |
8 | 897 | cmar | #define FORWARD 0 |
9 | #define BACK 1 |
||
10 | #define LEFT 2 |
||
11 | #define RIGHT 3 |
||
12 | #define LEFT_CURVE 4 |
||
13 | #define RIGHT_CURVE 5 |
||
14 | #define STOP 6 |
||
15 | #define VEL_UP 7 |
||
16 | #define VEL_DOWN 8 |
||
17 | |||
18 | void send_packet (char color, int dst_robot); |
||
19 | 901 | cmar | void packet_receive(char type, int source, unsigned char* packet, int length); |
20 | 897 | cmar | |
21 | 901 | cmar | PacketGroupHandler cntlHandler = {WL_CNTL_GROUP, NULL, NULL, NULL, NULL}; |
22 | PacketGroupHandler receiveHandler = {1, NULL, NULL, &packet_receive, NULL}; |
||
23 | 897 | cmar | |
24 | 901 | cmar | FILE *file; |
25 | |||
26 | 897 | cmar | int main(int argc, char *argv[]) { |
27 | if (argc != 2) { |
||
28 | printf("Usage: ./test <robot #>\n");
|
||
29 | return 1; |
||
30 | } |
||
31 | |||
32 | char c;
|
||
33 | int robot = atoi(argv[1]); |
||
34 | |||
35 | 901 | cmar | file = fopen("data_test.txt", "w"); |
36 | if (file == NULL) { |
||
37 | printf("fopen error\n");
|
||
38 | return 1; |
||
39 | } |
||
40 | 897 | cmar | initscr(); |
41 | printf("Beginning: robot=%d\r\n", robot);
|
||
42 | 336 | bcoltin | wl_set_com_port("/dev/ttyUSB0");
|
43 | 17 | bcoltin | wl_init(); |
44 | 336 | bcoltin | wl_set_channel(0xF);
|
45 | 897 | cmar | printf("Wireless initialized.\r\n");
|
46 | 901 | cmar | wl_register_packet_group(&cntlHandler); |
47 | wl_register_packet_group(&receiveHandler); |
||
48 | 897 | cmar | printf("Packet groups initialized.\r\n");
|
49 | fflush(stdout); |
||
50 | //while (1);
|
||
51 | while (1) { |
||
52 | 901 | cmar | wl_do(); |
53 | 897 | cmar | c = getch(); |
54 | printf("%c\r\n", c);
|
||
55 | fflush(stdout); |
||
56 | 901 | cmar | if (c=='0') |
57 | break;
|
||
58 | 897 | cmar | switch (c) {
|
59 | case 'w': |
||
60 | send_packet(FORWARD, robot); |
||
61 | break;
|
||
62 | case 's': |
||
63 | send_packet(BACK, robot); |
||
64 | break;
|
||
65 | case 'a': |
||
66 | send_packet(LEFT, robot); |
||
67 | break;
|
||
68 | case 'd': |
||
69 | send_packet(RIGHT, robot); |
||
70 | break;
|
||
71 | case 'q': |
||
72 | send_packet(LEFT_CURVE, robot); |
||
73 | break;
|
||
74 | case 'e': |
||
75 | send_packet(RIGHT_CURVE, robot); |
||
76 | break;
|
||
77 | case 'x': |
||
78 | send_packet(STOP, robot); |
||
79 | break;
|
||
80 | case ']': |
||
81 | send_packet(VEL_UP, robot); |
||
82 | break;
|
||
83 | case '[': |
||
84 | send_packet(VEL_DOWN, robot); |
||
85 | break;
|
||
86 | default:
|
||
87 | 901 | cmar | //send_packet(STOP, robot);
|
88 | 897 | cmar | break;
|
89 | } |
||
90 | 17 | bcoltin | } |
91 | 901 | cmar | fclose(file); |
92 | 17 | bcoltin | return 0; |
93 | } |
||
94 | |||
95 | 897 | cmar | void send_packet (char type, int dst_robot) { |
96 | wl_send_robot_to_robot_global_packet(WL_CNTL_GROUP, type, NULL, 0, dst_robot, 0); |
||
97 | //wl_send_global_packet(WL_CNTL_GROUP, type, NULL, 0, 0);
|
||
98 | } |
||
99 | 901 | cmar | |
100 | void packet_receive (char type, int source, unsigned char* packet, int length) { |
||
101 | fprintf(file, "received packet: ");
|
||
102 | int i, temp;
|
||
103 | for (i = 0; i < length; i += 2) { |
||
104 | temp = packet[i+1] << 8; |
||
105 | temp &= 0xFF00;
|
||
106 | temp |= packet[i]; |
||
107 | fprintf(file, "%d, ", temp);
|
||
108 | } |
||
109 | fprintf(file, "\n");
|
||
110 | } |