Revision 89
Did a lot of work on slam, the robot/computer should be able to read back all of the information from IR and the BOM's right now. No testing yet. Waiting for freshman... Mwa ha ha. Next step is the hard part, working on the actual mapping algorithm. Hopefully some freshman will know OpenGL or something.
computer_main.c | ||
---|---|---|
1 | 1 |
#include <stdio.h> |
2 | 2 |
#include <wireless.h> |
3 | 3 |
#include <unistd.h> |
4 |
#include <slam_defs.h> |
|
4 | 5 |
|
5 |
#define SLAM_PACKET_GROUP 11 |
|
6 |
#define CONTROL_PACKET_TYPE 'C' |
|
7 |
#define REQUEST_PACKET_TYPE 'R' |
|
8 |
#define ROBOT_RESPONSE 'I' |
|
9 |
|
|
10 |
#define MAX_ROBOTS 50 |
|
11 |
|
|
12 | 6 |
void handle_timeout(void); |
13 | 7 |
void handle_response(int frame, int received); |
14 | 8 |
void handle_receive(char type, int source, unsigned char* packet, int length); |
15 | 9 |
void unregister(void); |
16 | 10 |
void RobotData* getRobot(int id); |
17 | 11 |
|
18 |
typedef struct bom_node{ |
|
19 |
char value; |
|
20 |
short int id; |
|
21 |
BomNode* next; |
|
22 |
} BomNode; |
|
23 |
|
|
24 |
//If at some point we have more than 50 robots, this will fail. |
|
25 |
typedef struct robot_data{ |
|
26 |
int id; |
|
27 |
unsigned char IR[5]; |
|
28 |
RobotData* next; |
|
29 |
BomNode* bom_head; |
|
30 |
} RobotData; |
|
31 |
|
|
32 | 12 |
RobotData* head_bot; |
33 | 13 |
RobotData* current_bot; |
34 | 14 |
|
... | ... | |
45 | 25 |
wl_init(); |
46 | 26 |
wl_register_packet_group(pgh); |
47 | 27 |
wl_token_ring_register(); |
48 |
wl_token_ring_join(); |
|
49 |
wl_token_ring_set_bom_functions(NULL,NULL,NULL); |
|
28 |
//wl_token_ring_join();
|
|
29 |
//wl_token_ring_set_bom_functions(NULL,NULL,NULL);
|
|
50 | 30 |
|
51 | 31 |
while(1){ |
52 | 32 |
wl_do(); |
... | ... | |
56 | 36 |
while(wl_token_iterator_has_next()){ |
57 | 37 |
wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP,REQUEST_PACKET_TYPE, |
58 | 38 |
NULL,0,wl_token_iterator_next(),0); |
39 |
//head_bot will not be modified by the analyzer. |
|
59 | 40 |
usleep(50000); |
60 | 41 |
} |
42 |
slam_analyze(head_bot); |
|
61 | 43 |
} |
62 | 44 |
return 0; |
63 | 45 |
} |
... | ... | |
72 | 54 |
if(current_bot == NULL){ |
73 | 55 |
//Add the new robot to the front of the list. |
74 | 56 |
current_bot = malloc(sizeof(RobotData)); |
75 |
current_bot->next = head_bot; |
|
57 |
current_bot->next = head_bot; //Works even if head_bot is null.
|
|
76 | 58 |
head_bot = current_bot; |
77 | 59 |
} |
78 | 60 |
for(i=0;i<5;i++) |
... | ... | |
83 | 65 |
//Clear the current information. |
84 | 66 |
BomNode* head = current_bot->bom_head; |
85 | 67 |
if(head!=NULL){ |
86 |
BomNode* next = head-next; |
|
68 |
BomNode* next = head->next;
|
|
87 | 69 |
while(next!=NULL){ |
88 | 70 |
free(head); |
89 | 71 |
head = next; |
... | ... | |
94 | 76 |
//All of the memory allocated for the BomList is free. |
95 | 77 |
|
96 | 78 |
//The remainder of the packet is bom data. |
97 |
//TODO Finish this shit. |
|
98 |
BomNode* bom_node = malloc(sizeof(BomNode)); |
|
79 |
BomNode* bom_node; |
|
99 | 80 |
current_bot->bom_head = bom_node; |
81 |
//i still points to the first bom information spot at 5. |
|
100 | 82 |
while(i<length){ |
101 |
bom_node->id = (short int)packet[i]; |
|
102 |
i+=2; |
|
103 |
bom_node->value = packet[i]; |
|
104 |
i++; |
|
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; |
|
105 | 95 |
} |
106 | 96 |
} |
107 | 97 |
} |
... | ... | |
111 | 101 |
while(next->id != id && next->next!=NULL); |
112 | 102 |
|
113 | 103 |
if(next->id != id) return NULL; |
114 |
|
|
115 |
return next; |
|
104 |
else return next; |
|
116 | 105 |
} |
117 | 106 |
|
Also available in: Unified diff