root / trunk / code / projects / mapping / server / test.c @ 915
History | View | Annotate | Download (4.08 KB)
1 |
#include <stdlib.h> |
---|---|
2 |
#include <stdio.h> |
3 |
#include <curses.h> // you need to install the ncurses library |
4 |
#include "../../libwireless/lib/wireless.h" |
5 |
|
6 |
#define MAP 1 // packet group for receiving points |
7 |
#define POINT_RAW 1 // packet type for map data points w/ raw encoder data |
8 |
#define POINT_ODO 2 // packet type for map data points w/ odometry data |
9 |
|
10 |
#define WL_CNTL_GROUP 4 |
11 |
|
12 |
#define CNTL_FORWARD 0 |
13 |
#define CNTL_BACK 1 |
14 |
#define CNTL_LEFT 2 |
15 |
#define CNTL_RIGHT 3 |
16 |
#define CNTL_LEFT_CURVE 4 |
17 |
#define CNTL_RIGHT_CURVE 5 |
18 |
#define CNTL_STOP 6 |
19 |
#define CNTL_VEL_UP 7 |
20 |
#define CNTL_VEL_DOWN 8 |
21 |
|
22 |
void send_packet (char color, int dst_robot); |
23 |
void packet_receive(char type, int source, unsigned char* packet, int length); |
24 |
|
25 |
PacketGroupHandler cntlHandler = {WL_CNTL_GROUP, NULL, NULL, NULL, NULL}; |
26 |
PacketGroupHandler receiveHandler = {1, NULL, NULL, &packet_receive, NULL}; |
27 |
|
28 |
FILE *file; |
29 |
|
30 |
int main(int argc, char *argv[]) { |
31 |
if (argc != 2) { |
32 |
printf("Usage: ./test <robot #>\n");
|
33 |
return 1; |
34 |
} |
35 |
|
36 |
char c;
|
37 |
int robot = atoi(argv[1]); |
38 |
|
39 |
file = fopen("input.txt", "w"); |
40 |
if (file == NULL) { |
41 |
printf("fopen error\n");
|
42 |
return 1; |
43 |
} |
44 |
WINDOW* win = initscr(); |
45 |
nodelay(win, TRUE); |
46 |
printf("Beginning: robot=%d\r\n", robot);
|
47 |
wl_set_com_port("/dev/ttyUSB1");
|
48 |
wl_init(); |
49 |
wl_set_channel(0xF);
|
50 |
printf("Wireless initialized.\r\n");
|
51 |
wl_register_packet_group(&cntlHandler); |
52 |
wl_register_packet_group(&receiveHandler); |
53 |
printf("Packet groups initialized.\r\n");
|
54 |
fflush(stdout); |
55 |
//while (1);
|
56 |
while (1) { |
57 |
wl_do(); |
58 |
c = getch(); |
59 |
printf("%c\r\n", c);
|
60 |
fflush(stdout); |
61 |
if (c=='0') |
62 |
break;
|
63 |
switch (c) {
|
64 |
case 'w': |
65 |
send_packet(CNTL_FORWARD, robot); |
66 |
break;
|
67 |
case 's': |
68 |
send_packet(CNTL_BACK, robot); |
69 |
break;
|
70 |
case 'a': |
71 |
send_packet(CNTL_LEFT, robot); |
72 |
break;
|
73 |
case 'd': |
74 |
send_packet(CNTL_RIGHT, robot); |
75 |
break;
|
76 |
case 'q': |
77 |
send_packet(CNTL_LEFT_CURVE, robot); |
78 |
break;
|
79 |
case 'e': |
80 |
send_packet(CNTL_RIGHT_CURVE, robot); |
81 |
break;
|
82 |
case 'x': |
83 |
send_packet(CNTL_STOP, robot); |
84 |
break;
|
85 |
case ']': |
86 |
send_packet(CNTL_VEL_UP, robot); |
87 |
break;
|
88 |
case '[': |
89 |
send_packet(CNTL_VEL_DOWN, robot); |
90 |
break;
|
91 |
default:
|
92 |
//send_packet(CNTL_STOP, robot);
|
93 |
break;
|
94 |
} |
95 |
} |
96 |
fclose(file); |
97 |
delwin(win); |
98 |
endwin(); |
99 |
refresh(); |
100 |
return 0; |
101 |
} |
102 |
|
103 |
void send_packet (char type, int dst_robot) { |
104 |
wl_send_robot_to_robot_global_packet(WL_CNTL_GROUP, type, NULL, 0, dst_robot, 0); |
105 |
//wl_send_global_packet(WL_CNTL_GROUP, type, NULL, 0, 0);
|
106 |
} |
107 |
|
108 |
void packet_receive (char type, int source, unsigned char* packet, int length) { |
109 |
if (type == POINT_RAW) {
|
110 |
//expected input: enc_l enc_r IR1 IR2 IR3 IR4 IR5
|
111 |
int i, temp;
|
112 |
for (i = 0; i < length; i += 2) { |
113 |
temp = (char)packet[i+1] << 8; |
114 |
temp |= (char)packet[i];
|
115 |
fprintf(file, "%d ", temp);
|
116 |
} |
117 |
fprintf(file, "\n");
|
118 |
} |
119 |
else if (type == POINT_ODO) { |
120 |
// expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5
|
121 |
int x,y,ir1,ir2,ir3,ir4,ir5;
|
122 |
//double theta;
|
123 |
float theta;
|
124 |
long theta_temp;
|
125 |
x = (char)packet[0] + ((char)packet[1] << 8); |
126 |
y = (char)packet[2] + ((char)packet[3] << 8); |
127 |
theta_temp = (char)packet[4] + ((char)packet[5] << 8) |
128 |
+ ((char)packet[5] << 16) + ((char)packet[6] << 24); |
129 |
theta = (float)theta_temp;
|
130 |
ir1 = (char)packet[8] + ((char)packet[9] << 8); |
131 |
ir2 = (char)packet[10] + ((char)packet[11] << 8); |
132 |
ir3 = (char)packet[12] + ((char)packet[13] << 8); |
133 |
ir4 = (char)packet[14] + ((char)packet[15] << 8); |
134 |
ir5 = (char)packet[16] + ((char)packet[17] << 8); |
135 |
|
136 |
fprintf(file, "%d %d %f %d %d %d %d %d\n", x, y, theta, ir1, ir2, ir3, ir4, ir5);
|
137 |
} |
138 |
} |