root / branches / library_refactor / projects / mapping / matlab / testRobot / test.c @ 1390
History | View | Annotate | Download (2.7 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 | 1032 | justin | #define DATA_SERVER_IDENTIFY 5 |
15 | #define DATA_SERVER_REPLY 4 |
||
16 | 963 | justin | #define DATA_POINT 2 |
17 | #define DATA_REQUEST 3 |
||
18 | 953 | justin | |
19 | 1032 | justin | #define WL_CHANNEL 0xE |
20 | 1028 | justin | |
21 | 1032 | justin | int state; int velocity; |
22 | int server;
|
||
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 | 1032 | justin | server = -1;
|
34 | |||
35 | 953 | justin | /* initialize components and join the token ring */
|
36 | 1028 | justin | dragonfly_init(RANGE | BOM | COMM); |
37 | 1032 | justin | |
38 | usb_init(); usb_puts("usb turned on\r\n");
|
||
39 | |||
40 | //odometry_init();
|
||
41 | 953 | justin | |
42 | 1032 | justin | wl_init(); usb_puts("wireless turned on\r\n");
|
43 | 1028 | justin | wl_set_channel(WL_CHANNEL); |
44 | 953 | justin | wl_register_packet_group(&requestHandler); |
45 | |||
46 | wl_token_ring_register(); |
||
47 | 1028 | justin | wl_token_ring_set_bom_functions(transmit, bom_off, get_max_bom); |
48 | 953 | justin | wl_token_ring_join(); |
49 | 1032 | justin | usb_puts("token ring joined\r\n");
|
50 | 953 | justin | |
51 | 1032 | justin | char buf[500]; |
52 | |||
53 | int prescalar = 0; |
||
54 | |||
55 | 953 | justin | while(1) |
56 | { |
||
57 | wl_do(); |
||
58 | 1032 | justin | |
59 | if(!(++prescalar%1000)){ |
||
60 | sprintf(buf, "# Robots = %d, # Robots in matrix = %d\r\n",
|
||
61 | wl_token_get_robots_in_ring(), wl_token_get_num_robots()); |
||
62 | usb_puts(buf); |
||
63 | } |
||
64 | 953 | justin | } |
65 | |||
66 | 1032 | justin | usb_puts("Terminating\r\n");
|
67 | |||
68 | 953 | justin | wl_terminate(); |
69 | return 0; |
||
70 | } |
||
71 | |||
72 | void respond(char type, int source, unsigned char* packet, int length) { |
||
73 | 963 | justin | if(type==DATA_REQUEST){
|
74 | sprintf(buf, "received packet: %d\n", type);
|
||
75 | usb_puts(buf); |
||
76 | 953 | justin | |
77 | 963 | justin | int store[9]; |
78 | store[0] = 0; |
||
79 | store[1] = 0; |
||
80 | *((double*)(store + 2)) = 5.0; |
||
81 | 1032 | justin | store[4] = 125; /* IR1 range*/ |
82 | 963 | justin | store[5] = 125; // IR2 range |
83 | store[6] = 125; // IR3 range |
||
84 | store[7] = 125; // IR4 range |
||
85 | store[8] = 125; // IR5 range |
||
86 | 953 | justin | |
87 | 1028 | justin | char* data = (char*)store; |
88 | |||
89 | 963 | justin | wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, |
90 | 1028 | justin | DATA_POINT, data, 18, source, 0); |
91 | 1032 | justin | delay_ms(50);
|
92 | 963 | justin | } |
93 | 1032 | justin | else if(type==DATA_SERVER_REPLY){ |
94 | 1028 | justin | server = source; |
95 | } |
||
96 | 953 | justin | } |
97 | 960 | justin | |
98 | 1028 | justin | void transmit(){
|
99 | /*If a server hasn't declared itself, return.*/
|
||
100 | 1032 | justin | if(server == -1){ |
101 | wl_send_global_packet(MAP_REQUEST_GROUP, |
||
102 | DATA_SERVER_IDENTIFY, NULL, 0, 0); |
||
103 | delay_ms(50);
|
||
104 | } |
||
105 | else{
|
||
106 | 1028 | justin | |
107 | 1032 | justin | int store[9]; |
108 | store[0] = 0; //X |
||
109 | store[1] = 0; //Y |
||
110 | *((double*)(store + 2)) = 5.0; //Theta. |
||
111 | store[4] = 125; // IR1 range |
||
112 | store[5] = 125; // IR2 range |
||
113 | store[6] = 125; // IR3 range |
||
114 | store[7] = 125; // IR4 range |
||
115 | store[8] = 125; // IR5 range |
||
116 | 1028 | justin | |
117 | 1032 | justin | char* data = (char*)store; |
118 | 1028 | justin | |
119 | 1032 | justin | wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, |
120 | DATA_POINT, data, 18, server, 0); |
||
121 | delay_ms(50);
|
||
122 | |||
123 | bom_on(); |
||
124 | } |
||
125 | 1028 | justin | } |
126 | |||
127 |