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