Revision 541
Further work on SLAM...
| server_main.c | ||
|---|---|---|
| 5 | 5 |
#include "queue.h" |
| 6 | 6 |
#include "math.h" |
| 7 | 7 |
|
| 8 |
#define DEFAULT_WIDTH 50 |
|
| 9 |
#define DEFAULT_HEIGHT 50 |
|
| 10 |
#define MAX_MAPS 30 |
|
| 11 |
|
|
| 12 |
#define MAX_ID 20 |
|
| 13 |
|
|
| 8 | 14 |
typedef struct dataNode{
|
| 9 | 15 |
struct timeval* timeStamp; |
| 10 | 16 |
char type; |
| ... | ... | |
| 16 | 22 |
short robot_id; |
| 17 | 23 |
} Data; |
| 18 | 24 |
|
| 25 |
typedef struct robotScreenShot{
|
|
| 26 |
char robot_current_map; |
|
| 27 |
double robot_orientation; //-PI to PI |
|
| 28 |
int robot_x; |
|
| 29 |
int robot_y; |
|
| 30 |
} RobotScreenShot; |
|
| 31 |
|
|
| 19 | 32 |
/* |
| 20 | 33 |
This is the game plan if anyone is interested. |
| 21 | 34 |
Each robot will go for an unspecified amount of time, |
| ... | ... | |
| 50 | 63 |
but I think this is a solid method of doing SLAM. |
| 51 | 64 |
*/ |
| 52 | 65 |
|
| 53 |
/** |
|
| 54 |
* @brief An array of maps. |
|
| 55 |
*/ |
|
| 56 |
#define DEFAULT_WIDTH 50 |
|
| 57 |
#define DEFAULT_HEIGHT 50 |
|
| 58 |
//Indexed x,y |
|
| 59 |
int* map_array[DEFAULT_WIDTH][DEFAULT_HEIGHT]; |
|
| 60 | 66 |
|
| 61 |
/** |
|
| 62 |
* @brief An array of short's indicating which robot each map belongs to. |
|
| 63 |
*/ |
|
| 64 |
short* map_ids; |
|
| 65 | 67 |
|
| 66 |
|
|
| 67 | 68 |
/** |
| 68 |
* @brief Describes whether a map has been correlated to previous maps with a BOM reading.
|
|
| 69 |
* Map oriented data.
|
|
| 69 | 70 |
*/ |
| 70 |
char* map_correlated; |
|
| 71 |
int* map_array[MAX_MAPS][DEFAULT_WIDTH][DEFAULT_HEIGHT]; |
|
| 72 |
short map_ids[MAX_MAPS]; |
|
| 73 |
char map_correlated[MAX_MAPS]; |
|
| 71 | 74 |
|
| 75 |
/**Robot oriented data.**/ |
|
| 72 | 76 |
Queue* dataQueue; |
| 73 | 77 |
|
| 74 | 78 |
/*These functions simply add the data to the queue.*/ |
| ... | ... | |
| 76 | 80 |
void IR_handler(short id, short** data); |
| 77 | 81 |
void encoder_handler(short id, unsigned char** data); |
| 78 | 82 |
|
| 79 |
void bom_analyze(BomNode* head); |
|
| 80 |
void IR_analyze(short* data); |
|
| 81 |
void encoder_analyze(unsigned char* data); |
|
| 83 |
void bom_analyze(BomNode* head,short robot_id);
|
|
| 84 |
void IR_analyze(short* data, short robot_id);
|
|
| 85 |
void encoder_analyze(unsigned char* data, short robot_id);
|
|
| 82 | 86 |
|
| 83 | 87 |
|
| 84 | 88 |
void slam_analyze(void){
|
| ... | ... | |
| 89 | 93 |
switch(current_data->type){
|
| 90 | 94 |
|
| 91 | 95 |
case(BOM_TYPE): |
| 92 |
bom_analyze(current_data->data.u_b); |
|
| 96 |
bom_analyze(current_data->data.u_b,current_data->robot_id);
|
|
| 93 | 97 |
break; |
| 94 | 98 |
|
| 95 | 99 |
case(IR_TYPE): |
| 96 |
IR_analyze(current_data->data.u_s); |
|
| 100 |
IR_analyze(current_data->data.u_s,current_data->robot_id);
|
|
| 97 | 101 |
break; |
| 98 | 102 |
|
| 99 | 103 |
case(ENCODER_TYPE): |
| 100 |
encoder_analyze(current_data->data.u_c); |
|
| 104 |
encoder_analyze(current_data->data.u_c,current_data->robot_id);
|
|
| 101 | 105 |
break; |
| 102 | 106 |
} |
| 103 | 107 |
} |
| ... | ... | |
| 113 | 117 |
} |
| 114 | 118 |
|
| 115 | 119 |
|
| 116 |
void bom_analyze(BomNode* head){
|
|
| 117 |
|
|
| 120 |
void bom_analyze(BomNode* head, short robot_id){
|
|
| 121 |
BomNode* current_node = head; |
|
| 122 |
while(current_node!=NULL){
|
|
| 123 |
|
|
| 124 |
current_node = current_node->next; |
|
| 125 |
} |
|
| 118 | 126 |
} |
| 119 | 127 |
|
| 120 |
void IR_analyze(short* data){
|
|
| 128 |
void IR_analyze(short* data, short robot_id){
|
|
| 121 | 129 |
|
| 122 | 130 |
} |
| 123 | 131 |
|
| 124 |
void encoder_analyze(unsigned char* data){
|
|
| 132 |
void encoder_analyze(unsigned char* data, short robot_id){
|
|
| 125 | 133 |
|
| 126 | 134 |
} |
| 127 | 135 |
|
Also available in: Unified diff