root / branches / analog / code / projects / mapping / matlab / testRobot / test.c @ 1390
History | View | Annotate | Download (2.66 KB)
1 |
/** This test should just have the robot stay in place,
|
---|---|
2 |
* register the token ring, participate in the token ring,
|
3 |
* and send packets after flashing its BOM*/
|
4 |
|
5 |
#include <dragonfly_lib.h> |
6 |
#include <wireless.h> |
7 |
#include <wl_token_ring.h> |
8 |
#include <encoders.h> |
9 |
#include <bom.h> |
10 |
#include "odometry.h" |
11 |
|
12 |
#define MAP_REQUEST_GROUP 23 |
13 |
|
14 |
#define DATA_SERVER_IDENTIFY 5 |
15 |
#define DATA_SERVER_REPLY 4 |
16 |
#define DATA_POINT 2 |
17 |
#define DATA_REQUEST 3 |
18 |
|
19 |
#define WL_CHANNEL 0xE |
20 |
|
21 |
int state; int velocity; |
22 |
int server;
|
23 |
char buf[100]; |
24 |
|
25 |
void respond(char type, int source, unsigned char* packet, int length); |
26 |
void transmit(void); |
27 |
|
28 |
PacketGroupHandler requestHandler = {MAP_REQUEST_GROUP, |
29 |
NULL, NULL, respond, NULL}; |
30 |
|
31 |
int main(void) |
32 |
{ |
33 |
server = -1;
|
34 |
|
35 |
/* initialize components and join the token ring */
|
36 |
dragonfly_init(RANGE | BOM | COMM); |
37 |
|
38 |
usb_init(); usb_puts("usb turned on\r\n");
|
39 |
|
40 |
//odometry_init();
|
41 |
|
42 |
wl_init(); usb_puts("wireless turned on\r\n");
|
43 |
wl_set_channel(WL_CHANNEL); |
44 |
wl_register_packet_group(&requestHandler); |
45 |
|
46 |
wl_token_ring_register(); |
47 |
wl_token_ring_set_bom_functions(transmit, bom_off, get_max_bom); |
48 |
wl_token_ring_join(); |
49 |
usb_puts("token ring joined\r\n");
|
50 |
|
51 |
char buf[500]; |
52 |
|
53 |
int prescalar = 0; |
54 |
|
55 |
while(1) |
56 |
{ |
57 |
wl_do(); |
58 |
|
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 |
} |
65 |
|
66 |
usb_puts("Terminating\r\n");
|
67 |
|
68 |
wl_terminate(); |
69 |
return 0; |
70 |
} |
71 |
|
72 |
void respond(char type, int source, unsigned char* packet, int length) { |
73 |
if(type==DATA_REQUEST){
|
74 |
sprintf(buf, "received packet: %d\n", type);
|
75 |
usb_puts(buf); |
76 |
|
77 |
int store[9]; |
78 |
store[0] = 0; |
79 |
store[1] = 0; |
80 |
*((double*)(store + 2)) = 5.0; |
81 |
store[4] = 125; /* IR1 range*/ |
82 |
store[5] = 125; // IR2 range |
83 |
store[6] = 125; // IR3 range |
84 |
store[7] = 125; // IR4 range |
85 |
store[8] = 125; // IR5 range |
86 |
|
87 |
char* data = (char*)store; |
88 |
|
89 |
wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, |
90 |
DATA_POINT, data, 18, source, 0); |
91 |
} |
92 |
else if(type==DATA_SERVER_REPLY){ |
93 |
server = source; |
94 |
} |
95 |
} |
96 |
|
97 |
void transmit(){
|
98 |
/*If a server hasn't declared itself, return.*/
|
99 |
if(server == -1){ |
100 |
wl_send_global_packet(MAP_REQUEST_GROUP, |
101 |
DATA_SERVER_IDENTIFY, NULL, 0, 0); |
102 |
} |
103 |
else{
|
104 |
|
105 |
int store[9]; |
106 |
store[0] = 0; //X |
107 |
store[1] = 0; //Y |
108 |
*((double*)(store + 2)) = 5.0; //Theta. |
109 |
store[4] = 125; // IR1 range |
110 |
store[5] = 125; // IR2 range |
111 |
store[6] = 125; // IR3 range |
112 |
store[7] = 125; // IR4 range |
113 |
store[8] = 125; // IR5 range |
114 |
|
115 |
char* data = (char*)store; |
116 |
|
117 |
wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, |
118 |
DATA_POINT, data, 18, server, 0); |
119 |
|
120 |
bom_on(); |
121 |
} |
122 |
} |
123 |
|
124 |
|
125 |
|