Revision 915
updated robot and server code to use odometry. having problems with theta because it is a double (4 bytes on robot, 8 bytes on server)
| test.c | ||
|---|---|---|
| 3 | 3 |
#include <curses.h> // you need to install the ncurses library |
| 4 | 4 |
#include "../../libwireless/lib/wireless.h" |
| 5 | 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 |
|
|
| 6 | 10 |
#define WL_CNTL_GROUP 4 |
| 7 | 11 |
|
| 8 | 12 |
#define CNTL_FORWARD 0 |
| ... | ... | |
| 38 | 42 |
return 1; |
| 39 | 43 |
} |
| 40 | 44 |
WINDOW* win = initscr(); |
| 45 |
nodelay(win, TRUE); |
|
| 41 | 46 |
printf("Beginning: robot=%d\r\n", robot);
|
| 42 |
wl_set_com_port("/dev/ttyUSB0");
|
|
| 47 |
wl_set_com_port("/dev/ttyUSB1");
|
|
| 43 | 48 |
wl_init(); |
| 44 | 49 |
wl_set_channel(0xF); |
| 45 | 50 |
printf("Wireless initialized.\r\n");
|
| ... | ... | |
| 101 | 106 |
} |
| 102 | 107 |
|
| 103 | 108 |
void packet_receive (char type, int source, unsigned char* packet, int length) {
|
| 104 |
//expected input: x y theta IR1 IR2 IR3 IR4 IR5 |
|
| 105 |
int i, temp; |
|
| 106 |
for (i = 0; i < length; i += 2) {
|
|
| 107 |
temp = (char)packet[i+1] << 8; |
|
| 108 |
temp |= (char)packet[i]; |
|
| 109 |
fprintf(file, "%d ", temp); |
|
| 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"); |
|
| 110 | 118 |
} |
| 111 |
fprintf(file, "\n"); |
|
| 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 |
} |
|
| 112 | 138 |
} |
Also available in: Unified diff