root / trunk / code / projects / mapping / matlab / testRobot / test.c @ 960
History | View | Annotate | Download (1.6 KB)
1 | 960 | justin | /** This test should just have the robot stay in place,
|
---|---|---|---|
2 | * register the token ring, participate in the token ring,
|
||
3 | * and reply to packets when asked by the computer.*/
|
||
4 | |||
5 | 953 | justin | #include <dragonfly_lib.h> |
6 | #include <wireless.h> |
||
7 | #include <encoders.h> |
||
8 | |||
9 | 960 | justin | #define MAP_RECEIVE_GROUP 17 |
10 | #define MAP_REQUEST_GROUP 23 |
||
11 | 953 | justin | |
12 | #define POINT_RAW 1 // packet type for map data points w/ raw encoder data |
||
13 | #define POINT_ODO 2 // packet type for map data points w/ odometry data |
||
14 | |||
15 | #define WL_CNTL_GROUP 4 |
||
16 | |||
17 | int state;
|
||
18 | int velocity;
|
||
19 | char buf[100]; |
||
20 | |||
21 | void respond(char type, int source, unsigned char* packet, int length); |
||
22 | |||
23 | PacketGroupHandler receiveHandler = {MAP_REQUEST_GROUP, |
||
24 | NULL, NULL, NULL, NULL}; |
||
25 | |||
26 | PacketGroupHandler requestHandler = {MAP_RECEIVE_GROUP, |
||
27 | NULL, NULL, respond, NULL}; |
||
28 | |||
29 | int main(void) |
||
30 | { |
||
31 | /* initialize components and join the token ring */
|
||
32 | dragonfly_init(ALL_ON); |
||
33 | odometry_init(); |
||
34 | |||
35 | wl_init(); |
||
36 | wl_set_channel(0xE);
|
||
37 | wl_register_packet_group(&receiveHandler); |
||
38 | wl_register_packet_group(&requestHandler); |
||
39 | |||
40 | wl_token_ring_register(); |
||
41 | wl_token_ring_join(); |
||
42 | |||
43 | while(1) |
||
44 | { |
||
45 | wl_do(); |
||
46 | } |
||
47 | |||
48 | wl_terminate(); |
||
49 | return 0; |
||
50 | } |
||
51 | |||
52 | void respond(char type, int source, unsigned char* packet, int length) { |
||
53 | sprintf(buf, "received packet: %d\n", type);
|
||
54 | usb_puts(buf); |
||
55 | |||
56 | int store[9]; |
||
57 | store[0] = 0; |
||
58 | store[1] = 0; |
||
59 | *((double*)(store + 2)) = 5.0; |
||
60 | store[4] = 125; // IR1 range |
||
61 | store[5] = 125; // IR2 range |
||
62 | store[6] = 125; // IR3 range |
||
63 | store[7] = 125; // IR4 range |
||
64 | store[8] = 125; // IR5 range |
||
65 | |||
66 | wl_send_robot_to_robot_global_packet(MAP_RECEIVE_GROUP, |
||
67 | POINT_ODO, &store, 0, source, 0); |
||
68 | |||
69 | } |
||
70 | 960 | justin |