Revision 71
Worked on computer_main and robot_main,
Somewhat completed robot_main. Will need to clean up packet creation.
computer_main is nowhere near complete. Working on pulling in data from the robots.
trunk/code/projects/slam/.dep/robot_main.o.d | ||
---|---|---|
1 |
robot_main.o: robot_main.c \ |
|
2 |
../../../code/lib/include/libdragonfly/dragonfly_lib.h \ |
|
3 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/inttypes.h \ |
|
4 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/stdint.h \ |
|
5 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/stdio.h \ |
|
6 |
/opt/local/lib/gcc/avr/4.0.2/include/stdarg.h \ |
|
7 |
/opt/local/lib/gcc/avr/4.0.2/include/stddef.h \ |
|
8 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/stdlib.h \ |
|
9 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/io.h \ |
|
10 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/sfr_defs.h \ |
|
11 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/iom128.h \ |
|
12 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/portpins.h \ |
|
13 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/version.h \ |
|
14 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/interrupt.h \ |
|
15 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/util/delay.h \ |
|
16 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/util/twi.h \ |
|
17 |
../../../code/lib/include/libdragonfly/analog.h \ |
|
18 |
../../../code/lib/include/libdragonfly/dio.h \ |
|
19 |
../../../code/lib/include/libdragonfly/time.h \ |
|
20 |
../../../code/lib/include/libdragonfly/lcd.h \ |
|
21 |
../../../code/lib/include/libdragonfly/lights.h \ |
|
22 |
../../../code/lib/include/libdragonfly/motor.h \ |
|
23 |
../../../code/lib/include/libdragonfly/serial.h \ |
|
24 |
../../../code/lib/include/libdragonfly/buzzer.h \ |
|
25 |
../../../code/lib/include/libdragonfly/rangefinder.h \ |
|
26 |
../../../code/lib/include/libdragonfly/bom.h \ |
|
27 |
../../../code/lib/include/libdragonfly/move.h \ |
|
28 |
../../../code/lib/include/libwireless/wireless.h \ |
|
29 |
../../../code/lib/include/libwireless/wl_token_ring.h \ |
|
30 |
smart_run_around_fsm.h |
|
31 |
|
|
32 |
../../../code/lib/include/libdragonfly/dragonfly_lib.h: |
|
33 |
|
|
34 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/inttypes.h: |
|
35 |
|
|
36 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/stdint.h: |
|
37 |
|
|
38 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/stdio.h: |
|
39 |
|
|
40 |
/opt/local/lib/gcc/avr/4.0.2/include/stdarg.h: |
|
41 |
|
|
42 |
/opt/local/lib/gcc/avr/4.0.2/include/stddef.h: |
|
43 |
|
|
44 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/stdlib.h: |
|
45 |
|
|
46 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/io.h: |
|
47 |
|
|
48 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/sfr_defs.h: |
|
49 |
|
|
50 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/iom128.h: |
|
51 |
|
|
52 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/portpins.h: |
|
53 |
|
|
54 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/version.h: |
|
55 |
|
|
56 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/avr/interrupt.h: |
|
57 |
|
|
58 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/util/delay.h: |
|
59 |
|
|
60 |
/opt/local/lib/gcc/avr/4.0.2/../../../../avr/include/util/twi.h: |
|
61 |
|
|
62 |
../../../code/lib/include/libdragonfly/analog.h: |
|
63 |
|
|
64 |
../../../code/lib/include/libdragonfly/dio.h: |
|
65 |
|
|
66 |
../../../code/lib/include/libdragonfly/time.h: |
|
67 |
|
|
68 |
../../../code/lib/include/libdragonfly/lcd.h: |
|
69 |
|
|
70 |
../../../code/lib/include/libdragonfly/lights.h: |
|
71 |
|
|
72 |
../../../code/lib/include/libdragonfly/motor.h: |
|
73 |
|
|
74 |
../../../code/lib/include/libdragonfly/serial.h: |
|
75 |
|
|
76 |
../../../code/lib/include/libdragonfly/buzzer.h: |
|
77 |
|
|
78 |
../../../code/lib/include/libdragonfly/rangefinder.h: |
|
79 |
|
|
80 |
../../../code/lib/include/libdragonfly/bom.h: |
|
81 |
|
|
82 |
../../../code/lib/include/libdragonfly/move.h: |
|
83 |
|
|
84 |
../../../code/lib/include/libwireless/wireless.h: |
|
85 |
|
|
86 |
../../../code/lib/include/libwireless/wl_token_ring.h: |
|
87 |
|
|
88 |
smart_run_around_fsm.h: |
trunk/code/projects/slam/computer_main.c | ||
---|---|---|
1 |
#include <dragonfly_lib.h> |
|
1 |
#include <stdio.h> |
|
2 |
#include <wireless.h> |
|
3 |
#include <unistd.h> |
|
2 | 4 |
|
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 |
void handle_timeout(void); |
|
13 |
void handle_response(int frame, int received); |
|
14 |
void handle_receive(char type, int source, unsigned char* packet, int length); |
|
15 |
void unregister(void); |
|
16 |
void RobotData* getRobot(int id); |
|
17 |
|
|
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 |
RobotData* head_bot; |
|
33 |
RobotData* current_bot; |
|
34 |
|
|
3 | 35 |
int main(void) |
4 | 36 |
{ |
5 |
dragonfly_init(ALL_ON); |
|
37 |
PacketGroupHandler* pgh = malloc(sizeof(PacketGroupHandler)); |
|
38 |
pgh->groupCode = SLAM_PACKET_GROUP; |
|
39 |
pgh->timeout_handler = handle_timeout; |
|
40 |
pgh->handle_response = handle_response; |
|
41 |
pgh->handle_receive = handle_receive; |
|
42 |
pgh->unregister = unregister; |
|
43 |
|
|
44 |
|
|
45 |
wl_init(); |
|
46 |
wl_register_packet_group(pgh); |
|
47 |
wl_token_ring_register(); |
|
48 |
wl_token_ring_join(); |
|
49 |
wl_token_ring_set_bom_functions(NULL,NULL,NULL); |
|
50 |
|
|
51 |
while(1){ |
|
52 |
wl_do(); |
|
53 |
wl_token_iterator_begin() |
|
54 |
|
|
55 |
//Iterate through the robots in the token ring. |
|
56 |
while(wl_token_iterator_has_next()){ |
|
57 |
wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP,REQUEST_PACKET_TYPE, |
|
58 |
NULL,0,wl_token_iterator_next(),0); |
|
59 |
usleep(50000); |
|
60 |
} |
|
61 |
} |
|
6 | 62 |
return 0; |
7 | 63 |
} |
8 | 64 |
|
65 |
void handle_timeout(void){} |
|
66 |
void handle_response(int frame, int received){} |
|
67 |
|
|
68 |
void handle_receive(char type, int source, unsigned char* packet, int length){ |
|
69 |
if(type == ROBOT_RESPONSE){ |
|
70 |
int i; |
|
71 |
current_bot = getRobot(source); |
|
72 |
if(current_bot == NULL){ |
|
73 |
//Add the new robot to the front of the list. |
|
74 |
current_bot = malloc(sizeof(RobotData)); |
|
75 |
current_bot->next = head_bot; |
|
76 |
head_bot = current_bot; |
|
77 |
} |
|
78 |
for(i=0;i<5;i++) |
|
79 |
current_bot->IR[i] = packet[i]; |
|
80 |
i++; //i=5 |
|
81 |
|
|
82 |
//Bom Handling. |
|
83 |
//Clear the current information. |
|
84 |
BomNode* head = current_bot->bom_head; |
|
85 |
if(head!=NULL){ |
|
86 |
BomNode* next = head-next; |
|
87 |
while(next!=NULL){ |
|
88 |
free(head); |
|
89 |
head = next; |
|
90 |
next = head->next; |
|
91 |
} |
|
92 |
free(head); |
|
93 |
} |
|
94 |
//All of the memory allocated for the BomList is free. |
|
95 |
|
|
96 |
//The remainder of the packet is bom data. |
|
97 |
//TODO Finish this shit. |
|
98 |
BomNode* bom_node = malloc(sizeof(BomNode)); |
|
99 |
current_bot->bom_head = bom_node; |
|
100 |
while(i<length){ |
|
101 |
bom_node->id = (short int)packet[i]; |
|
102 |
i+=2; |
|
103 |
bom_node->value = packet[i]; |
|
104 |
i++; |
|
105 |
} |
|
106 |
} |
|
107 |
} |
|
108 |
|
|
109 |
RobotData* getRobot(int id){ |
|
110 |
RobotData* next = head_bot; |
|
111 |
while(next->id != id && next->next!=NULL); |
|
112 |
|
|
113 |
if(next->id != id) return NULL; |
|
114 |
|
|
115 |
return next; |
|
116 |
} |
|
117 |
|
trunk/code/projects/slam/robot_main.c | ||
---|---|---|
27 | 27 |
void receive_handle(char type, int source, unsigned char* packet, int length); |
28 | 28 |
void unregister(void); |
29 | 29 |
|
30 |
char* make_request_packet(char* packet);
|
|
30 |
int make_request_packet(char* packet);
|
|
31 | 31 |
|
32 | 32 |
int IR[5]; |
33 | 33 |
|
... | ... | |
72 | 72 |
bom_off(); |
73 | 73 |
} |
74 | 74 |
|
75 |
//These can probably be NULL.. |
|
76 |
|
|
75 | 77 |
void timeout_handle(void){ |
76 |
wl_token_ring_join(); |
|
78 |
|
|
77 | 79 |
} |
78 | 80 |
|
79 | 81 |
void response_handle(int frame, int received){ |
... | ... | |
83 | 85 |
void receive_handle(char type, int source, unsigned char* packet, int length){ |
84 | 86 |
int i; |
85 | 87 |
if(type==REQUEST_PACKET_TYPE){ |
86 |
char packet[REQUEST_PACKET_SIZE]; |
|
87 | 88 |
//Send a message back to our computer overlord. |
88 |
int len= make_request_packet(packet); |
|
89 |
|
|
89 |
char buf[PACKET_BUFFER_SIZE]; |
|
90 |
int len = make_request_packet(&buf); |
|
91 |
char packet[len]; |
|
92 |
for(i=0; i< len ; i++){ |
|
93 |
packet[i]=buf[i]; |
|
94 |
} |
|
95 |
|
|
90 | 96 |
wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP, ROBOT_RESPONSE, packet, len, source , 0); |
91 | 97 |
} |
92 | 98 |
else if(type == CONTROL_PACKET_TYPE){ |
93 | 99 |
for(i=0;i<length;i++){ |
94 |
|
|
100 |
|
|
95 | 101 |
} |
96 | 102 |
} |
97 | 103 |
} |
98 | 104 |
|
99 |
int make_request_packet(char* packet){ |
|
100 |
int size = wl_token_get_num_robots(); |
|
105 |
|
|
106 |
/** |
|
107 |
|
|
108 |
Packets are structured {IR1, IR2, IR3, IR4, IR5, |
|
109 |
BomID1, ... , BomValue1, BomID2, ... , BomValue2, etc.} |
|
110 |
Total size is 5 + 3*numRobots. So Xbee should support |
|
111 |
up to 31 robots before we need to restructure the |
|
112 |
communication between the computer and the robot. |
|
113 |
|
|
114 |
*/ |
|
115 |
int make_request_packet(char** packet){ |
|
101 | 116 |
int i,j; |
102 | 117 |
for(i=0;i<5;i++){ |
103 |
packet[i] = IR[i];
|
|
118 |
(*packet)[i] = IR[i];
|
|
104 | 119 |
} |
105 |
for(i=0;i<size;i++){ |
|
106 |
for(j=0;j<size;j++){ |
|
107 |
// wl_token_get_sensor_reading(int source, int dest); |
|
108 |
|
|
109 |
} |
|
120 |
i=5; |
|
121 |
wl_token_iterator_begin(); |
|
122 |
int id = wl_get_xbee_id(); |
|
123 |
|
|
124 |
while(wl_token_iterator_has_next()){ |
|
125 |
int robot_in_view = wl_token_iterator_next(); |
|
126 |
|
|
127 |
//Robot int's are two bytes. |
|
128 |
(*packet)[i] = robot_in_view; |
|
129 |
i+=2; |
|
130 |
|
|
131 |
int bom_number = wl_token_get_sensor_reading(id,robot_in_view); |
|
132 |
//Bom reading of 255 denotes an unseen node. |
|
133 |
if(bom_number<0) bom_number = 255; |
|
134 |
bom_number %= 256; |
|
135 |
char bom_value = (char) bom_number; |
|
136 |
(*packet)[i] = bom_value; |
|
137 |
i++; |
|
110 | 138 |
} |
111 |
return size; |
|
139 |
//Size is one bom byte towards every robot, and 5 IR bytes. |
|
140 |
return wl_token_get_num_robots()+5; |
|
112 | 141 |
} |
113 | 142 |
|
trunk/code/projects/slam/Makefile | ||
---|---|---|
3 | 3 |
# |
4 | 4 |
|
5 | 5 |
# Relative path to the root directory (containing lib directory) |
6 |
#ifndef COLONYROOT
|
|
6 |
ifndef COLONYROOT |
|
7 | 7 |
COLONYROOT = ../../.. |
8 |
#endif
|
|
8 |
endif |
|
9 | 9 |
|
10 | 10 |
# Target file name (without extension). |
11 | 11 |
TARGET = robot_main |
Also available in: Unified diff