root / trunk / code / projects / mapping / auto / robot_main.c @ 1124
History | View | Annotate | Download (1.41 KB)
1 |
#include <dragonfly_lib.h> |
---|---|
2 |
#include <wireless.h> |
3 |
#include <encoders.h> |
4 |
#include "odometry.h" |
5 |
|
6 |
#define POLLING_INTERVAL 50 //interval in ms to get sensor data |
7 |
#define MAP 1 // packet group for sending data points |
8 |
|
9 |
char buf[100]; |
10 |
|
11 |
int main(void) |
12 |
{ |
13 |
/* initialize components and join the token ring */
|
14 |
dragonfly_init(ALL_ON); |
15 |
odometry_init(); |
16 |
orb_enable(); |
17 |
wl_init(); |
18 |
wl_set_channel(0xE);
|
19 |
|
20 |
int x, y; // use when sending odometry data |
21 |
double theta;
|
22 |
|
23 |
int store[9]; |
24 |
char *send; // for transmission |
25 |
|
26 |
while(1) |
27 |
{ |
28 |
wl_do(); |
29 |
|
30 |
delay_ms(POLLING_INTERVAL); /* until it's time to grab map data */
|
31 |
|
32 |
x = odometry_dx(); // data gathered
|
33 |
y = odometry_dy(); |
34 |
theta = odometry_angle(); |
35 |
|
36 |
store[0] = x; // position |
37 |
store[1] = y;
|
38 |
*((double*)(store + 2)) = theta; // calculated angle (8 bytes) |
39 |
store[4] = range_read_distance(IR1); // IR1 range |
40 |
store[5] = range_read_distance(IR2); // IR2 range |
41 |
store[6] = range_read_distance(IR3); // IR3 range |
42 |
store[7] = range_read_distance(IR4); // IR4 range |
43 |
store[8] = range_read_distance(IR5); // IR5 range |
44 |
sprintf(buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1], |
45 |
*((double *)(store+2)), store[4], store[5], store[6], |
46 |
store[7], store[8]); |
47 |
usb_puts(buf); // send data to USB for debugging
|
48 |
|
49 |
send = (char*) store; // convert to char for wireless |
50 |
|
51 |
wl_send_global_packet(MAP, MAP, send, 18, 0); |
52 |
} |
53 |
|
54 |
wl_terminate(); |
55 |
return 0; |
56 |
} |
57 |
|