Revision 965
Cleaned up test.c and robot_main.c in mapping.
| test.c | ||
|---|---|---|
| 24 | 24 |
void packet_receive(char type, int source, unsigned char* packet, int length); |
| 25 | 25 |
|
| 26 | 26 |
PacketGroupHandler cntlHandler = {WL_CNTL_GROUP, NULL, NULL, NULL, NULL};
|
| 27 |
PacketGroupHandler receiveHandler = {1, NULL, NULL, &packet_receive, NULL};
|
|
| 27 |
PacketGroupHandler receiveHandler = {MAP, NULL, NULL, &packet_receive, NULL};
|
|
| 28 | 28 |
|
| 29 | 29 |
FILE *file; |
| 30 | 30 |
int parent_running = 1; |
| ... | ... | |
| 108 | 108 |
void send_packet (char type, int dst_robot) {
|
| 109 | 109 |
wl_send_robot_to_robot_global_packet(WL_CNTL_GROUP, type, |
| 110 | 110 |
NULL, 0, dst_robot, 0); |
| 111 |
//wl_send_global_packet(WL_CNTL_GROUP, type, NULL, 0, 0); |
|
| 112 | 111 |
} |
| 113 | 112 |
|
| 114 | 113 |
void packet_receive (char type, int source, unsigned char* packet, int length) {
|
| 115 |
if (type == POINT_RAW) {
|
|
| 116 |
//expected input: enc_l enc_r IR1 IR2 IR3 IR4 IR5 |
|
| 117 |
int i, temp; |
|
| 118 |
for (i = 0; i < length; i += 2) {
|
|
| 119 |
temp = (char)packet[i+1] << 8; |
|
| 120 |
temp |= (char)packet[i]; |
|
| 121 |
fprintf(file, "%d ", temp); |
|
| 122 |
} |
|
| 123 |
fprintf(file, "\n"); |
|
| 124 |
} |
|
| 125 |
//else if (type == POINT_ODO) {
|
|
| 126 |
//printf("received packet!\r\n");
|
|
| 127 |
// expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5 |
|
| 128 |
short x,y,ir1,ir2,ir3,ir4,ir5; |
|
| 129 |
//int theta_temp; |
|
| 130 |
x = ((short)packet[1] << 8) | (short)packet[0]; |
|
| 131 |
y = ((short)packet[3] << 8) | (short)packet[2]; |
|
| 132 |
ir1 = ((short)packet[9] << 8) | (short)packet[8]; |
|
| 133 |
ir2 = ((short)packet[11] << 8) | (short)packet[10]; |
|
| 134 |
ir3 = ((short)packet[13] << 8) | (short)packet[12]; |
|
| 135 |
ir4 = ((short)packet[15] << 8) | (short)packet[14]; |
|
| 136 |
ir5 = ((short)packet[17] << 8) | (short)packet[16]; |
|
| 114 |
short x,y,ir1,ir2,ir3,ir4,ir5; |
|
| 137 | 115 |
|
| 138 |
char tarr[] = {packet[4],packet[5],packet[6],packet[7]};
|
|
| 139 |
float *theta_ptr = (float *)tarr; |
|
| 140 |
float theta = *theta_ptr; |
|
| 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]; |
|
| 141 | 124 |
|
| 142 |
//fprintf(file, "theta = %f\n", theta); |
|
| 143 |
fprintf(file, "%d %d %f %d %d %d %d %d\n", x, y, theta, |
|
| 144 |
ir1, ir2, ir3, ir4, ir5); |
|
| 145 |
//printf("%d %d %f %d %d %d %d %d\n", x, y, theta,
|
|
| 146 |
// ir1, ir2, ir3, ir4, ir5); |
|
| 147 |
//fflush(stdout); |
|
| 148 |
//} |
|
| 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); |
|
| 149 | 131 |
} |
Also available in: Unified diff