Revision 525
Did some slam work, trying to get together a good statistical way of reading range values.
There will be experimenting.... boring horrible experimenting
| server_main.c | ||
|---|---|---|
| 1 |
#include <stdio.h> |
|
| 2 |
#include <stdlib.h> |
|
| 3 |
#include <time.h> |
|
| 1 | 4 |
#include "data_requests.h" |
| 2 | 5 |
#include "queue.h" |
| 6 |
#include "math.h" |
|
| 3 | 7 |
|
| 4 |
|
|
| 5 | 8 |
typedef struct dataNode{
|
| 6 |
long timeStamp;
|
|
| 9 |
struct timeval* timeStamp;
|
|
| 7 | 10 |
char type; |
| 8 | 11 |
union{
|
| 9 | 12 |
BomNode* u_b; |
| 10 | 13 |
short* u_s; |
| 11 | 14 |
unsigned char* u_c; |
| 12 | 15 |
} data; |
| 16 |
short robot_id; |
|
| 13 | 17 |
} Data; |
| 14 | 18 |
|
| 15 | 19 |
/* |
| ... | ... | |
| 49 | 53 |
/** |
| 50 | 54 |
* @brief An array of maps. |
| 51 | 55 |
*/ |
| 52 |
int*** map_array; |
|
| 56 |
#define DEFAULT_WIDTH 50 |
|
| 57 |
#define DEFAULT_HEIGHT 50 |
|
| 58 |
//Indexed x,y |
|
| 59 |
int* map_array[DEFAULT_WIDTH][DEFAULT_HEIGHT]; |
|
| 53 | 60 |
|
| 54 | 61 |
/** |
| 55 | 62 |
* @brief An array of short's indicating which robot each map belongs to. |
| 56 | 63 |
*/ |
| 57 |
int* map_ids[];
|
|
| 64 |
short* map_ids;
|
|
| 58 | 65 |
|
| 66 |
|
|
| 67 |
/** |
|
| 68 |
* @brief Describes whether a map has been correlated to previous maps with a BOM reading. |
|
| 69 |
*/ |
|
| 70 |
char* map_correlated; |
|
| 71 |
|
|
| 59 | 72 |
Queue* dataQueue; |
| 60 | 73 |
|
| 61 | 74 |
/*These functions simply add the data to the queue.*/ |
| 62 |
void bom_handler(BomNode** head); |
|
| 63 |
void ir_handler(short** data);
|
|
| 64 |
void encoder_handler(unsigned char** data); |
|
| 75 |
void bom_handler(short id, BomNode** head);
|
|
| 76 |
void IR_handler(short id, short** data);
|
|
| 77 |
void encoder_handler(short id, unsigned char** data);
|
|
| 65 | 78 |
|
| 66 |
void slam_analyze(); |
|
| 67 | 79 |
void bom_analyze(BomNode* head); |
| 68 |
void ir_analyze(short* data);
|
|
| 80 |
void IR_analyze(short* data);
|
|
| 69 | 81 |
void encoder_analyze(unsigned char* data); |
| 70 | 82 |
|
| 71 |
int main(void){
|
|
| 72 |
data_requests_init(bom_handler,IR_handler,encoder_handler); |
|
| 73 |
dataQueue = queue_create(); |
|
| 74 |
slam_analyze(); |
|
| 75 |
} |
|
| 76 | 83 |
|
| 77 |
void slam_analyze(){
|
|
| 84 |
void slam_analyze(void){
|
|
| 78 | 85 |
Data* current_data; |
| 79 | 86 |
while(1){
|
| 80 | 87 |
if(!queue_is_empty(dataQueue)){
|
| ... | ... | |
| 86 | 93 |
break; |
| 87 | 94 |
|
| 88 | 95 |
case(IR_TYPE): |
| 89 |
ir_analyze(current_data->data.u_s);
|
|
| 96 |
IR_analyze(current_data->data.u_s);
|
|
| 90 | 97 |
break; |
| 91 | 98 |
|
| 92 | 99 |
case(ENCODER_TYPE): |
| ... | ... | |
| 97 | 104 |
} |
| 98 | 105 |
} |
| 99 | 106 |
|
| 107 |
|
|
| 108 |
int main(void){
|
|
| 109 |
data_requests_init(bom_handler,IR_handler,encoder_handler); |
|
| 110 |
dataQueue = queue_create(); |
|
| 111 |
slam_analyze(); |
|
| 112 |
return -1; |
|
| 113 |
} |
|
| 114 |
|
|
| 115 |
|
|
| 100 | 116 |
void bom_analyze(BomNode* head){
|
| 101 | 117 |
|
| 102 | 118 |
} |
| 103 | 119 |
|
| 104 |
void ir_analyze(short* data){
|
|
| 120 |
void IR_analyze(short* data){
|
|
| 105 | 121 |
|
| 106 | 122 |
} |
| 107 | 123 |
|
| ... | ... | |
| 111 | 127 |
|
| 112 | 128 |
/*The packet handling functions for data requests.*/ |
| 113 | 129 |
|
| 114 |
void bom_handler(BomNode** head){
|
|
| 130 |
void bom_handler(short id, BomNode** head){
|
|
| 115 | 131 |
Data* newData = malloc(sizeof(Data)); |
| 116 |
newData->data = (*head); |
|
| 117 |
//newData->robot_id = ????? |
|
| 118 |
//newData->time = |
|
| 132 |
newData->type = BOM_TYPE; |
|
| 133 |
newData->data.u_b = (*head); |
|
| 134 |
newData->robot_id = id; |
|
| 135 |
|
|
| 136 |
gettimeofday(&newData->timeStamp,NULL); |
|
| 137 |
|
|
| 119 | 138 |
queue_add(dataQueue,(void*)(newData)); |
| 120 | 139 |
} |
| 121 | 140 |
|
| 122 |
void ir_handler(short** data){
|
|
| 123 |
//etc. |
|
| 141 |
void IR_handler(short id, short** data){
|
|
| 142 |
Data* newData = malloc(sizeof(Data)); |
|
| 143 |
newData->type = IR_TYPE; |
|
| 144 |
newData->data.u_s = (*data); |
|
| 145 |
newData->robot_id = id; |
|
| 146 |
|
|
| 147 |
gettimeofday(&newData->timeStamp,NULL); |
|
| 148 |
|
|
| 149 |
queue_add(dataQueue,(void*)(newData)); |
|
| 150 |
|
|
| 124 | 151 |
} |
| 125 | 152 |
|
| 126 |
void encoder_handler(unsigned char** data){
|
|
| 153 |
void encoder_handler(short id, unsigned char** data){
|
|
| 154 |
Data* newData = malloc(sizeof(Data)); |
|
| 155 |
newData->type = ENCODER_TYPE; |
|
| 156 |
newData->data.u_c = (*data); |
|
| 157 |
newData->robot_id = id; |
|
| 158 |
|
|
| 159 |
gettimeofday(&newData->timeStamp,NULL); |
|
| 160 |
|
|
| 161 |
queue_add(dataQueue,(void*)(newData)); |
|
| 162 |
} |
|
| 127 | 163 |
|
| 164 |
|
|
| 165 |
/** |
|
| 166 |
* @brief Rotates a map by radians radians. |
|
| 167 |
* Weak version... Uses doubles and a secondary map is probably extra slow. |
|
| 168 |
* Can this be done in place? |
|
| 169 |
* @param map The map to be rotated. |
|
| 170 |
*/ |
|
| 171 |
void rotate_map(int*** map, double radians){
|
|
| 172 |
int i,j; |
|
| 173 |
double iprime, jprime, xoffset, yoffset; |
|
| 174 |
int i2,j2; |
|
| 175 |
int new_map[DEFAULT_WIDTH][DEFAULT_HEIGHT]; |
|
| 176 |
|
|
| 177 |
for(i=0;i<DEFAULT_WIDTH;i++){
|
|
| 178 |
for(j=0;j<DEFAULT_WIDTH;i++){
|
|
| 179 |
new_map[i][j] = 0; |
|
| 180 |
} |
|
| 181 |
} |
|
| 182 |
|
|
| 183 |
for(i=0;i<DEFAULT_WIDTH;i++){
|
|
| 184 |
for(j=0;j<DEFAULT_WIDTH;i++){
|
|
| 185 |
iprime = i*cos(radians)-j*sin(radians); |
|
| 186 |
jprime = i*sin(radians)+j*cos(radians); |
|
| 187 |
|
|
| 188 |
//Smear the value over a range near i2,j2 The new values may need to be normalized TODO Figure that out.; |
|
| 189 |
i2 = (int)iprime; |
|
| 190 |
j2 = (int)jprime; |
|
| 191 |
xoffset = iprime - i2; |
|
| 192 |
yoffset = jprime - j2; |
|
| 193 |
|
|
| 194 |
//If the new coordinates are legal... |
|
| 195 |
if(i2 < DEFAULT_WIDTH && i2 >= 0 && j2 < DEFAULT_HEIGHT && j2 >= 0) |
|
| 196 |
new_map[i2][j2] += (int)(sqrt(xoffset*xoffset + yoffset*yoffset)*(double)((*map)[i][j])); |
|
| 197 |
|
|
| 198 |
if(i2 + 1 < DEFAULT_WIDTH && i2 + 1 >= 0 && j2 < DEFAULT_HEIGHT && j2 >= 0) |
|
| 199 |
new_map[i2+1][j2] += (int)(sqrt((1-xoffset)*(1-xoffset) + yoffset*yoffset)*(double)((*map)[i][j])); |
|
| 200 |
|
|
| 201 |
if(i2 < DEFAULT_WIDTH && i2 >= 0 && j2 + 1 < DEFAULT_HEIGHT && j2 + 1 >= 0) |
|
| 202 |
new_map[i2][j2+1] += (int)(sqrt(xoffset*xoffset + (1-yoffset)*(1-yoffset))*(double)((*map)[i][j])); |
|
| 203 |
|
|
| 204 |
if(i2 + 1 < DEFAULT_WIDTH && i2 + 1 >= 0 && j2 + 1 < DEFAULT_HEIGHT && j2 + 1 >= 0) |
|
| 205 |
new_map[i2+1][j2+1] += (int)(sqrt((1-xoffset)*(1-xoffset) + (1-yoffset)*(1-yoffset))*(double)((*map)[i][j])); |
|
| 206 |
} |
|
| 207 |
} |
|
| 208 |
|
|
| 209 |
//Incompatible types? FIXME |
|
| 210 |
(*map) = new_map; |
|
| 211 |
return; |
|
| 128 | 212 |
} |
| 213 |
|
|
| 214 |
|
|
Also available in: Unified diff