Revision 965
Cleaned up test.c and robot_main.c in mapping.
robot_main.c | ||
---|---|---|
5 | 5 |
|
6 | 6 |
#define POLLING_INTERVAL 50 //interval in ms to get sensor data |
7 | 7 |
#define MAP 1 // packet group for sending data 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 |
|
8 |
#define POINT_ODO 2 |
|
10 | 9 |
|
11 | 10 |
#define WL_CNTL_GROUP 4 |
12 | 11 |
|
... | ... | |
28 | 27 |
void set_motion (void); |
29 | 28 |
|
30 | 29 |
PacketGroupHandler packetHandler = {WL_CNTL_GROUP, NULL, NULL, &packet_receive, NULL}; |
31 |
PacketGroupHandler love_handle = {MAP, NULL, NULL, NULL, NULL}; |
|
32 | 30 |
|
33 | 31 |
int main(void) |
34 | 32 |
{ |
35 | 33 |
/* initialize components and join the token ring */ |
36 | 34 |
dragonfly_init(ALL_ON); |
37 |
//range_init(); |
|
38 |
//encoders_init(); |
|
39 | 35 |
odometry_init(); |
36 |
orb_enable(); |
|
40 | 37 |
wl_init(); |
41 | 38 |
wl_set_channel(0xE); |
42 |
wl_register_packet_group(&love_handle); |
|
43 | 39 |
wl_register_packet_group(&packetHandler); |
44 |
orb_enable(); |
|
45 |
//data_response_init(); |
|
46 |
//wl_token_ring_register(); |
|
47 |
//wl_token_ring_join(); |
|
48 |
//run_around_init(); |
|
49 |
|
|
50 |
//usb_puti(xbee_get_address()); |
|
51 |
//return 0; |
|
52 |
|
|
40 |
|
|
53 | 41 |
velocity = 190; |
54 | 42 |
state = CNTL_STOP; |
55 | 43 |
|
... | ... | |
62 | 50 |
while(1) |
63 | 51 |
{ |
64 | 52 |
wl_do(); |
65 |
//run_around_FSM(); /* traverse the environment and avoid obstacles ... */ |
|
66 | 53 |
delay_ms(POLLING_INTERVAL); /* until it's time to grab map data */ |
67 | 54 |
x = odometry_dx(); // use when sending odometry data |
68 | 55 |
y = odometry_dy(); |
69 | 56 |
theta = odometry_angle(); |
70 |
//store[0] = encoder_get_dx(LEFT); // left encoder distance |
|
71 |
//store[1] = encoder_get_dx(RIGHT); // right encoder distance |
|
72 | 57 |
store[0] = x; |
73 |
store[1] = y;
|
|
74 |
*((double*)(store + 2)) = theta;
|
|
75 |
store[4] = range_read_distance(IR1); // IR1 range
|
|
58 |
store[1] = y;
|
|
59 |
*((double*)(store + 2)) = theta;
|
|
60 |
store[4] = range_read_distance(IR1); // IR1 range
|
|
76 | 61 |
store[5] = range_read_distance(IR2); // IR2 range |
77 | 62 |
store[6] = range_read_distance(IR3); // IR3 range |
78 | 63 |
store[7] = range_read_distance(IR4); // IR4 range |
79 | 64 |
store[8] = range_read_distance(IR5); // IR5 range |
80 |
sprintf( buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1],
|
|
81 |
*((double *)(store+2)), store[4], store[5], store[6],
|
|
82 |
store[7], store[8] );
|
|
83 |
usb_puts(buf);
|
|
65 |
sprintf( buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1],
|
|
66 |
*((double *)(store+2)), store[4], store[5], store[6],
|
|
67 |
store[7], store[8] );
|
|
68 |
usb_puts(buf);
|
|
84 | 69 |
|
85 | 70 |
send = (char*) store; |
86 | 71 |
|
87 |
// send point w/ raw encoder data |
|
88 |
// wl_send_global_packet(MAP, POINT_RAW, send, 14, 0); |
|
89 |
|
|
90 |
// send point w/ odometry data |
|
91 | 72 |
wl_send_global_packet(MAP, POINT_ODO, send, 18, 0); |
92 |
|
|
93 | 73 |
} |
94 | 74 |
|
95 | 75 |
wl_terminate(); |
Also available in: Unified diff