root / trunk / code / projects / slam / computer_main.c @ 89
History | View | Annotate | Download (2.8 KB)
1 |
#include <stdio.h> |
---|---|
2 |
#include <wireless.h> |
3 |
#include <unistd.h> |
4 |
#include <slam_defs.h> |
5 |
|
6 |
void handle_timeout(void); |
7 |
void handle_response(int frame, int received); |
8 |
void handle_receive(char type, int source, unsigned char* packet, int length); |
9 |
void unregister(void); |
10 |
void RobotData* getRobot(int id); |
11 |
|
12 |
RobotData* head_bot; |
13 |
RobotData* current_bot; |
14 |
|
15 |
int main(void) |
16 |
{ |
17 |
PacketGroupHandler* pgh = malloc(sizeof(PacketGroupHandler));
|
18 |
pgh->groupCode = SLAM_PACKET_GROUP; |
19 |
pgh->timeout_handler = handle_timeout; |
20 |
pgh->handle_response = handle_response; |
21 |
pgh->handle_receive = handle_receive; |
22 |
pgh->unregister = unregister; |
23 |
|
24 |
|
25 |
wl_init(); |
26 |
wl_register_packet_group(pgh); |
27 |
wl_token_ring_register(); |
28 |
//wl_token_ring_join();
|
29 |
//wl_token_ring_set_bom_functions(NULL,NULL,NULL);
|
30 |
|
31 |
while(1){ |
32 |
wl_do(); |
33 |
wl_token_iterator_begin() |
34 |
|
35 |
//Iterate through the robots in the token ring.
|
36 |
while(wl_token_iterator_has_next()){
|
37 |
wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP,REQUEST_PACKET_TYPE, |
38 |
NULL,0,wl_token_iterator_next(),0); |
39 |
//head_bot will not be modified by the analyzer.
|
40 |
usleep(50000);
|
41 |
} |
42 |
slam_analyze(head_bot); |
43 |
} |
44 |
return 0; |
45 |
} |
46 |
|
47 |
void handle_timeout(void){} |
48 |
void handle_response(int frame, int received){} |
49 |
|
50 |
void handle_receive(char type, int source, unsigned char* packet, int length){ |
51 |
if(type == ROBOT_RESPONSE){
|
52 |
int i;
|
53 |
current_bot = getRobot(source); |
54 |
if(current_bot == NULL){ |
55 |
//Add the new robot to the front of the list.
|
56 |
current_bot = malloc(sizeof(RobotData));
|
57 |
current_bot->next = head_bot; //Works even if head_bot is null.
|
58 |
head_bot = current_bot; |
59 |
} |
60 |
for(i=0;i<5;i++) |
61 |
current_bot->IR[i] = packet[i]; |
62 |
i++; //i=5
|
63 |
|
64 |
//Bom Handling.
|
65 |
//Clear the current information.
|
66 |
BomNode* head = current_bot->bom_head; |
67 |
if(head!=NULL){ |
68 |
BomNode* next = head->next; |
69 |
while(next!=NULL){ |
70 |
free(head); |
71 |
head = next; |
72 |
next = head->next; |
73 |
} |
74 |
free(head); |
75 |
} |
76 |
//All of the memory allocated for the BomList is free.
|
77 |
|
78 |
//The remainder of the packet is bom data.
|
79 |
BomNode* bom_node; |
80 |
current_bot->bom_head = bom_node; |
81 |
//i still points to the first bom information spot at 5.
|
82 |
while(i<length){
|
83 |
bom_node = malloc(sizeof BomNode);
|
84 |
bom_node->id = (short int)((short)packet[i++]<<8 + packet[i++]); |
85 |
if(i>=length){
|
86 |
printf("Packet not constructed correctly. Terminate. \n");
|
87 |
exit(EXIT_FAILURE); |
88 |
} |
89 |
bom_node->value = packet[i++]; |
90 |
if(i>=length){
|
91 |
printf("Packet not constructed correctly. Terminate. \n");
|
92 |
exit(EXIT_FAILURE); |
93 |
} |
94 |
bom_node = bom_node->next; |
95 |
} |
96 |
} |
97 |
} |
98 |
|
99 |
RobotData* getRobot(int id){
|
100 |
RobotData* next = head_bot; |
101 |
while(next->id != id && next->next!=NULL); |
102 |
|
103 |
if(next->id != id) return NULL; |
104 |
else return next; |
105 |
} |
106 |
|