root / trunk / code / projects / mapping / server / test.c @ 965
History | View | Annotate | Download (3.99 KB)
1 | 908 | cmar | #include <stdlib.h> |
---|---|---|---|
2 | #include <stdio.h> |
||
3 | 911 | cmar | #include <curses.h> // you need to install the ncurses library |
4 | 908 | cmar | #include "../../libwireless/lib/wireless.h" |
5 | 917 | cmar | #include <unistd.h> |
6 | 908 | cmar | |
7 | 915 | cmar | #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 | 908 | cmar | #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 | 965 | alevkoy | PacketGroupHandler receiveHandler = {MAP, NULL, NULL, &packet_receive, NULL}; |
28 | 908 | cmar | |
29 | FILE *file; |
||
30 | 917 | cmar | int parent_running = 1; |
31 | 908 | cmar | |
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 | 928 | cmar | wl_set_com_port("/dev/ttyUSB1");
|
43 | 908 | cmar | wl_init(); |
44 | 928 | cmar | wl_set_channel(0xE);
|
45 | 908 | cmar | 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 | 917 | cmar | |
51 | file = fopen("input.txt", "w"); |
||
52 | if (file == NULL) { |
||
53 | printf("fopen error\n");
|
||
54 | return 1; |
||
55 | 908 | cmar | } |
56 | 917 | cmar | |
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 | 908 | cmar | return 0; |
106 | } |
||
107 | |||
108 | void send_packet (char type, int dst_robot) { |
||
109 | 917 | cmar | wl_send_robot_to_robot_global_packet(WL_CNTL_GROUP, type, |
110 | NULL, 0, dst_robot, 0); |
||
111 | 908 | cmar | } |
112 | |||
113 | void packet_receive (char type, int source, unsigned char* packet, int length) { |
||
114 | 965 | alevkoy | short x,y,ir1,ir2,ir3,ir4,ir5;
|
115 | 915 | cmar | |
116 | 965 | alevkoy | /* 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 | 917 | cmar | |
125 | 965 | alevkoy | 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 | 908 | cmar | } |