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