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.
| trunk/code/projects/slam/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 |
|
| trunk/code/projects/slam/robot_main.c | ||
|---|---|---|
| 1 |
#include <dragonfly_lib.h> |
|
| 2 |
#include <wireless.h> |
|
| 3 |
#include <wl_token_ring.h> |
|
| 4 |
#include <smart_run_around_fsm.h> |
|
| 1 |
#include "dragonfly_lib.h" |
|
| 2 |
#include "wireless.h" |
|
| 3 |
#include "wl_token_ring.h" |
|
| 4 |
#include "smart_run_around_fsm.h" |
|
| 5 |
#include "slam_defs.h" |
|
| 5 | 6 |
|
| 6 | 7 |
#define BOM_POST_DELAY 50 |
| 7 |
#define SLAM_PACKET_GROUP 11 |
|
| 8 |
#define CONTROL_PACKET_TYPE 'C' |
|
| 9 |
#define REQUEST_PACKET_TYPE 'R' |
|
| 10 |
#define ROBOT_RESPONSE 'I' |
|
| 8 |
#define PACKET_BUFFER_SIZE 101 |
|
| 11 | 9 |
|
| 12 |
//REQUEST_PACKET_ORGANIZATION |
|
| 13 |
#define REQUEST_PACKET_SIZE 50 |
|
| 14 |
#define IR_1_LOC 0 |
|
| 15 |
#define IR_2_LOC 1 |
|
| 16 |
#define IR_3_LOC 2 |
|
| 17 |
#define IR_4_LOC 3 |
|
| 18 |
#define IR_5_LOC 4 |
|
| 19 |
#define BOM_START_LOC 5 |
|
| 20 |
|
|
| 21 | 10 |
void bom_pre(void); |
| 22 | 11 |
void bom_post(void); |
| 23 | 12 |
|
| ... | ... | |
| 27 | 16 |
void receive_handle(char type, int source, unsigned char* packet, int length); |
| 28 | 17 |
void unregister(void); |
| 29 | 18 |
|
| 30 |
int make_request_packet(char* packet);
|
|
| 19 |
int make_request_packet(char** packet);
|
|
| 31 | 20 |
|
| 32 | 21 |
int IR[5]; |
| 33 | 22 |
|
| ... | ... | |
| 85 | 74 |
void receive_handle(char type, int source, unsigned char* packet, int length){
|
| 86 | 75 |
int i; |
| 87 | 76 |
if(type==REQUEST_PACKET_TYPE){
|
| 88 |
//Send a message back to our computer overlord. |
|
| 77 |
|
|
| 78 |
/*Send a message back to our computer overlord.*/ |
|
| 89 | 79 |
char buf[PACKET_BUFFER_SIZE]; |
| 90 | 80 |
int len = make_request_packet(&buf); |
| 91 | 81 |
char packet[len]; |
Also available in: Unified diff