root / trunk / code / projects / mapping / matlab / testRobot / test.c @ 1028
History | View | Annotate | Download (2.14 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 | 1028 | justin | #include <wl_token_ring.h> |
8 | 953 | justin | #include <encoders.h> |
9 | 1028 | justin | #include <bom.h> |
10 | 963 | justin | #include "odometry.h" |
11 | 953 | justin | |
12 | 960 | justin | #define MAP_REQUEST_GROUP 23 |
13 | 953 | justin | |
14 | 1028 | justin | #define DATA_INIT 4 |
15 | 963 | justin | #define DATA_POINT 2 |
16 | #define DATA_REQUEST 3 |
||
17 | 953 | justin | |
18 | 1028 | justin | #define WL_CHANNEL = 0xE |
19 | |||
20 | 953 | justin | int state;
|
21 | int velocity;
|
||
22 | 1028 | justin | int server = -1; |
23 | 953 | justin | char buf[100]; |
24 | |||
25 | void respond(char type, int source, unsigned char* packet, int length); |
||
26 | 1028 | justin | void transmit(void); |
27 | 953 | justin | |
28 | 963 | justin | PacketGroupHandler requestHandler = {MAP_REQUEST_GROUP, |
29 | 953 | justin | NULL, NULL, respond, NULL}; |
30 | |||
31 | int main(void) |
||
32 | { |
||
33 | /* initialize components and join the token ring */
|
||
34 | 1028 | justin | dragonfly_init(RANGE | BOM | COMM); |
35 | 953 | justin | odometry_init(); |
36 | |||
37 | wl_init(); |
||
38 | 1028 | justin | wl_set_channel(WL_CHANNEL); |
39 | 953 | justin | wl_register_packet_group(&requestHandler); |
40 | |||
41 | wl_token_ring_register(); |
||
42 | 1028 | justin | wl_token_ring_set_bom_functions(transmit, bom_off, get_max_bom); |
43 | 953 | justin | wl_token_ring_join(); |
44 | |||
45 | while(1) |
||
46 | { |
||
47 | wl_do(); |
||
48 | } |
||
49 | |||
50 | wl_terminate(); |
||
51 | return 0; |
||
52 | } |
||
53 | |||
54 | void respond(char type, int source, unsigned char* packet, int length) { |
||
55 | 963 | justin | if(type==DATA_REQUEST){
|
56 | sprintf(buf, "received packet: %d\n", type);
|
||
57 | usb_puts(buf); |
||
58 | 953 | justin | |
59 | 963 | justin | int store[9]; |
60 | store[0] = 0; |
||
61 | store[1] = 0; |
||
62 | *((double*)(store + 2)) = 5.0; |
||
63 | store[4] = 125; // IR1 range |
||
64 | store[5] = 125; // IR2 range |
||
65 | store[6] = 125; // IR3 range |
||
66 | store[7] = 125; // IR4 range |
||
67 | store[8] = 125; // IR5 range |
||
68 | 953 | justin | |
69 | 1028 | justin | char* data = (char*)store; |
70 | |||
71 | 963 | justin | wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, |
72 | 1028 | justin | DATA_POINT, data, 18, source, 0); |
73 | 963 | justin | } |
74 | 1028 | justin | else if(type==DATA_INIT){ |
75 | server = source; |
||
76 | } |
||
77 | 953 | justin | } |
78 | 960 | justin | |
79 | 1028 | justin | void transmit(){
|
80 | bom_on(); |
||
81 | 960 | justin | |
82 | 1028 | justin | /*If a server hasn't declared itself, return.*/
|
83 | if(server == -1) return; |
||
84 | |||
85 | int store[9]; |
||
86 | store[0] = 0; //X |
||
87 | store[1] = 0; //Y |
||
88 | *((double*)(store + 2)) = 5.0; //Theta. |
||
89 | store[4] = 125; // IR1 range |
||
90 | store[5] = 125; // IR2 range |
||
91 | store[6] = 125; // IR3 range |
||
92 | store[7] = 125; // IR4 range |
||
93 | store[8] = 125; // IR5 range |
||
94 | |||
95 | char* data = (char*)store; |
||
96 | |||
97 | wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, |
||
98 | DATA_POINT, data, 18, server, 0); |
||
99 | } |
||
100 | |||
101 |