root / trunk / code / projects / mapping / auto / robot_main.c @ 1124
History | View | Annotate | Download (1.41 KB)
1 | 898 | alevkoy | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include <wireless.h> |
||
3 | 912 | alevkoy | #include <encoders.h> |
4 | 915 | cmar | #include "odometry.h" |
5 | 722 | justin | |
6 | 917 | cmar | #define POLLING_INTERVAL 50 //interval in ms to get sensor data |
7 | 898 | alevkoy | #define MAP 1 // packet group for sending data points |
8 | |||
9 | 900 | cmar | char buf[100]; |
10 | |||
11 | 898 | alevkoy | int main(void) |
12 | { |
||
13 | /* initialize components and join the token ring */
|
||
14 | 899 | alevkoy | dragonfly_init(ALL_ON); |
15 | 915 | cmar | odometry_init(); |
16 | 965 | alevkoy | orb_enable(); |
17 | 899 | alevkoy | wl_init(); |
18 | 925 | cmar | wl_set_channel(0xE);
|
19 | 965 | alevkoy | |
20 | 915 | cmar | int x, y; // use when sending odometry data |
21 | double theta;
|
||
22 | 898 | alevkoy | |
23 | 915 | cmar | int store[9]; |
24 | 899 | alevkoy | char *send; // for transmission |
25 | 898 | alevkoy | |
26 | while(1) |
||
27 | { |
||
28 | 1124 | alevkoy | wl_do(); |
29 | 898 | alevkoy | |
30 | 1124 | alevkoy | delay_ms(POLLING_INTERVAL); /* until it's time to grab map data */
|
31 | 898 | alevkoy | |
32 | 1124 | alevkoy | 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 | 898 | alevkoy | } |
53 | |||
54 | 899 | alevkoy | wl_terminate(); |
55 | 898 | alevkoy | return 0; |
56 | 722 | justin | } |