Project

General

Profile

Revision 761

Svn gods be kind.

View differences:

trunk/code/projects/slam/slam_analyzer.c
1
#include "slam_analyzer.h"
2
#include "slam_defs.h"
3
#include <stdio.h>
4
#include <stdlib.h>
5

  
6

  
7

  
8
void slam_analyze(RobotData* head){
9
  //The good stuff.
10

  
11
}
12

  
13

  
14

  
15
void slam_print_data(RobotData* head){
16
  if(head==NULL) printf("No data to display.\n");
17
  RobotData* iter = head;
18
  BomNode* bomIter;
19
  char i;
20
  while(iter!= NULL){
21
    bomIter = iter->bom_head;
22
    printf("Robot %x:\n",iter->id);
23
    for(i=0;i<5;i++)
24
      printf("\tSharp IR %i : %i\n\n",i,iter->IR[i]);
25
    if(BomIter == NULL){
26
        printf("No Bom Data to Display\n\n\n");
27
    }
28
    else{
29
      while(bomIter!=NULL){
30
        printf("\tBom reading for robot %x : %i",BomIter->id, BomIter->value);
31
        bomIter = bomIter->next;
32
      }
33
    }
34
  }
35
}
trunk/code/projects/slam/slam_defs.h
1
#define SLAM_PACKET_GROUP 11
2
#define CONTROL_PACKET_TYPE 'C'
3
#define REQUEST_PACKET_TYPE 'R'
4
#define ROBOT_RESPONSE 'I'
5

  
6
#define MAX_ROBOTS 50 
7

  
8
//Request Packet organization.
9
#define IR_1_LOC 0
10
#define IR_2_LOC 1
11
#define IR_3_LOC 2
12
#define IR_4_LOC 3
13
#define IR_5_LOC 4
14
#define BOM_START_LOC 5
15

  
16
typedef struct bom_node{
17
  char value;
18
  short int id;
19
  struct bom_node* next;
20
} BomNode;
21

  
22
typedef struct robot_data{
23
  short int id; 
24
  unsigned char IR[5]; 
25
  struct robot_data* next;
26
  struct bom_node* bom_head;
27
} RobotData;
28

  
29
RobotData* head_bot;
30
RobotData* current_bot;
trunk/code/projects/slam/computer_main.c
1
#include <stdio.h>
2
#include <wireless.h>
3
#include <unistd.h>
4
#include <slam_defs.h>
5

  
6
void handle_timeout(void);
7
void handle_response(int frame, int received);
8
void handle_receive(char type, int source, unsigned char* packet, int length);
9
void unregister(void);
10
void RobotData* getRobot(int id);
11

  
12
RobotData* head_bot;
13
RobotData* current_bot;
14

  
15
int main(void)
16
{
17
  PacketGroupHandler* pgh = malloc(sizeof(PacketGroupHandler)); 
18
  pgh->groupCode = SLAM_PACKET_GROUP;
19
  pgh->timeout_handler = handle_timeout;
20
  pgh->handle_response = handle_response;
21
  pgh->handle_receive = handle_receive;
22
  pgh->unregister = unregister;
23

  
24
  
25
  wl_init();
26
  wl_register_packet_group(pgh);
27
  wl_token_ring_register();
28
  //wl_token_ring_join();
29
  //wl_token_ring_set_bom_functions(NULL,NULL,NULL);
30

  
31
  while(1){
32
    wl_do();
33
    wl_token_iterator_begin() 
34
    
35
    //Iterate through the robots in the token ring. 
36
    while(wl_token_iterator_has_next()){
37
      wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP,REQUEST_PACKET_TYPE,
38
        NULL,0,wl_token_iterator_next(),0);   
39
      //head_bot will not be modified by the analyzer. 
40
      usleep(50000); 
41
    }
42
    slam_analyze(head_bot);
43
  }
44
	return 0;
45
}
46

  
47
void handle_timeout(void){}
48
void handle_response(int frame, int received){}
49

  
50
void handle_receive(char type, int source, unsigned char* packet, int length){
51
  if(type == ROBOT_RESPONSE){
52
    int i; 
53
    current_bot = getRobot(source);
54
    if(current_bot == NULL){
55
      //Add the new robot to the front of the list.
56
      current_bot = malloc(sizeof(RobotData));
57
      current_bot->next = head_bot; //Works even if head_bot is null.
58
      head_bot = current_bot;
59
    }
60
    for(i=0;i<5;i++)
61
      current_bot->IR[i] = packet[i];
62
    i++; //i=5 
63
   
64
    //Bom Handling.
65
    //Clear the current information.
66
    BomNode* head = current_bot->bom_head;
67
    if(head!=NULL){
68
      BomNode* next = head->next;
69
      while(next!=NULL){
70
        free(head);
71
        head = next;
72
        next = head->next;
73
      }
74
      free(head);
75
    }
76
    //All of the memory allocated for the BomList is free.
77

  
78
    //The remainder of the packet is bom data. 
79
    BomNode* bom_node;
80
    current_bot->bom_head = bom_node;
81
    //i still points to the first bom information spot at 5. 
82
    while(i<length){
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;
95
    }
96
  }
97
}
98

  
99
RobotData* getRobot(int id){
100
  RobotData* next = head_bot; 
101
  while(next->id != id && next->next!=NULL);
102
  
103
  if(next->id != id) return NULL;
104
  else return next;
105
}
106

  
trunk/code/projects/slam/slam_analyzer.h
1
#include <stdio.h>
2
#include <stdlib.h>
3
#include <slam_defs.h>
4

  
5
void slam_analyze(RobotData* head);
6
void slam_print_data(RobotData* head);
trunk/code/projects/slam/smart_run_around_fsm.c
1
#include "dragonfly_lib.h"
2
#include "smart_run_around_fsm.h"
3

  
4
/*A simple behavior for navigating in an environment, i.e. avoiding walls and getting stuck.
5
Could be better at not getting stuck.
6

  
7
Latest revision only has two accessible states: move and reverse.
8
*/
9

  
10

  
11
void run_around_init(void)
12
{
13
  range_init();
14
  analog_init();
15
  motors_init();
16
  orb_init();
17
  orb_enable();
18
  usb_init();
19
 
20
  /*Start in the default state, MOVING*/ 
21
  avoid_state=MOVING;
22
  /*Set timers to their maximum values.*/
23
  crazy_count=CRAZY_MAX;
24
  backup_count=0; 
25
  pControl=0;
26
  
27
  /*Initialize distances to zero.*/ 
28
  d1=0; d2=0; d3=0; d4=0; d5=0;
29
  
30
  orb_set_color(GREEN);
31

  
32
}
33

  
34
/*The main function, call this to update states as frequently as possible.*/
35
void run_around_FSM(void) {
36
  /*Default to moving.*/ 
37
  avoid_state=MOVING;
38
  
39
  /*The following lines ensure that undefined (-1) values
40
  will not update the distances.*/ 
41
  int temp;
42
  
43
  temp=range_read_distance(IR1);
44
  d1=(temp == -1) ? d1 : temp;
45
  
46
  temp=range_read_distance(IR2);
47
  d2=(temp == -1) ? d2 : temp;
48
  
49
  temp=range_read_distance(IR3);
50
  d3=(temp == -1) ? d3 : temp;
51
  
52
  temp=range_read_distance(IR4);
53
  d4=(temp == -1) ? d4 : temp;
54
  
55
  temp=range_read_distance(IR5);
56
  d5=(temp == -1) ? d5 : temp;
57
  
58
  /*If the crazy count is in it's >>3 range, it acts crazy.*/
59
  if(crazy_count<=(CRAZY_MAX>>3))
60
  {
61
    avoid_state=CRAZY;
62
    crazy_count--;
63
    
64
    if(crazy_count<0) crazy_count=CRAZY_MAX;
65
    
66
    evaluate_state();
67
    return;
68
  }
69
  
70
  //Checks the forward distance to see if it should back up, if so...state backwards.
71
  if((d2!=-1)&&(d2 >150)){
72
      backup_count=BACKUP_MAX;
73
      avoid_state=BACKWARDS;
74
      evaluate_state();
75
      return;
76
  }
77
 
78
  if(backup_count<BACKUP_MAX){
79
    avoid_state=BACKWARDS; 
80
    if(backup_count<0)
81
      backup_count=BACKUP_MAX;
82
    evaluate_state();
83
    return;
84
  }
85
  
86
  /*Should evaluate an expression from -255 to 255 to pass to move.*/
87
  pControl= ((d3-d1) + (d4-d5)) >> TURN_CONSTANT;
88
  
89
  if(pControl>PCONTROL_CRAZY_LIMIT || pControl<-PCONTROL_CRAZY_LIMIT) crazy_count--;
90
  /*i.e. if you really want to turn for an extended period of time...you're probably stuck.*/
91

  
92
  /*Debug stuff:*/
93
  /*usb_puts("pControl evaluating: ");
94
  usb_puti(pControl);
95
  usb_puts("\n\r");
96
  usb_puts("IR1: ");
97
  usb_puti(d1);
98
  usb_puts(" IR2: ");
99
  usb_puti(d2);
100
  usb_puts(" IR3: ");
101
  usb_puti(d3);
102
  usb_puts(" IR4: ");
103
  usb_puti(d4);
104
  usb_puts(" IR5: ");
105
  usb_puti(d5);
106
  usb_puts("\n\r");*/
107
  
108
  evaluate_state();
109
}
110

  
111

  
112
//Acts on state change.
113
void evaluate_state(){
114
    switch(avoid_state){
115
    case(MOVING): orb_set_color(GREEN);
116
      move(STRAIT_SPEED,pControl);
117
      break;
118
    
119
    case(BACKWARDS): orb_set_color(ORANGE);
120
      move(-STRAIT_SPEED,0);
121
      break;
122
      
123
    case(CRAZY): orb_set_color(RED);
124
      /*TODO: Implement a crazy state.*/
125
      move(STRAIT_SPEED,pControl);
126
      break;
127
      
128
    default:
129
      /*Should never get here, go strait.*/
130
      move(100,0); orb_set_color(BLUE);
131
      break;
132
  }
133
}
134

  
135

  
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"
5
#include "slam_defs.h"
6

  
7
#define BOM_POST_DELAY 50
8
#define PACKET_BUFFER_SIZE 101 
9

  
10
void bom_pre(void);
11
void bom_post(void);
12

  
13
//Packet Handler Functions
14
void timeout_handle(void);
15
void response_handle(int frame, int received);
16
void receive_handle(char type, int source, unsigned char* packet, int length);
17
void unregister(void);
18

  
19
int make_request_packet(char** packet);
20

  
21
int IR[5];
22

  
23
int main(void)
24
{
25

  
26
  dragonfly_init(ALL_ON);
27
  
28
  wl_init();
29
 
30
  wl_token_ring_register();
31
  wl_token_ring_join();
32
  wl_token_ring_set_bom_functions(bom_pre, bom_post, get_max_bom);
33

  
34
  PacketGroupHandler packet_group_handler;
35
  packet_group_handler.groupCode=SLAM_PACKET_GROUP;
36
  packet_group_handler.timeout_handler=timeout_handle;
37
  packet_group_handler.handle_response=response_handle;
38
  packet_group_handler.handle_receive=receive_handle;
39
  packet_group_handler.unregister=unregister;
40

  
41
  wl_register_packet_group(&packet_group_handler);
42
  
43
  int i;
44
  while(1){
45
    wl_do();
46
    run_around_FSM(); 
47
    for(i=0;i<5;i++){
48
      IR[i]=range_read_distance(i);
49
    }
50
  }
51

  
52
  wl_unregister_packet_group(&packet_group_handler); 
53
}
54

  
55
void bom_pre(void){
56
    bom_on();
57
}
58

  
59
void bom_post(void){
60
    delay_ms(BOM_POST_DELAY);
61
    bom_off();
62
}
63

  
64
//These can probably be NULL..
65

  
66
void timeout_handle(void){
67

  
68
}
69

  
70
void response_handle(int frame, int received){
71
  
72
}
73

  
74
void receive_handle(char type, int source, unsigned char* packet, int length){
75
  int i; 
76
  if(type==REQUEST_PACKET_TYPE){
77
    
78
    /*Send a message back to our computer overlord.*/
79
    char buf[PACKET_BUFFER_SIZE]; 
80
    int len = make_request_packet(&buf);
81
    char packet[len];
82
    for(i=0; i< len ; i++){
83
      packet[i]=buf[i];
84
    } 
85
    
86
    wl_send_robot_to_robot_packet(SLAM_PACKET_GROUP, ROBOT_RESPONSE, packet, len, source , 0);
87
  }
88
  else if(type == CONTROL_PACKET_TYPE){
89
    for(i=0;i<length;i++){
90
      
91
    }
92
  }
93
}
94

  
95

  
96
/**
97

  
98
Packets are structured {IR1, IR2, IR3, IR4, IR5,
99
BomID1, ... , BomValue1, BomID2, ... , BomValue2, etc.} 
100
Total size is 5 + 3*numRobots. So Xbee should support
101
up to 31 robots before we need to restructure the 
102
communication between the computer and the robot.
103

  
104
*/
105
int make_request_packet(char** packet){
106
  int i,j;
107
  for(i=0;i<5;i++){
108
      (*packet)[i] = IR[i];
109
  } 
110
  i=5; 
111
  wl_token_iterator_begin();
112
  int id = wl_get_xbee_id();  
113
  
114
  while(wl_token_iterator_has_next()){
115
    int robot_in_view = wl_token_iterator_next(); 
116
    
117
    //Robot int's are two bytes. 
118
    (*packet)[i] = robot_in_view; 
119
    i+=2;
120
    
121
    int bom_number = wl_token_get_sensor_reading(id,robot_in_view);
122
    //Bom reading of 255 denotes an unseen node. 
123
    if(bom_number<0) bom_number = 255; 
124
    bom_number %= 256;
125
    char bom_value = (char) bom_number;
126
    (*packet)[i] = bom_value;
127
    i++;
128
  }
129
  //Size is one bom byte towards every robot, and 5 IR bytes. 
130
  return wl_token_get_num_robots()+5;
131
}
132

  
trunk/code/projects/slam/smart_run_around_fsm.h
1
//Obstacle Avoid Numbers
2

  
3

  
4
#ifndef _RUN_AROUND_FSM_H_
5
#define _RUN_AROUND_FSM_H_
6

  
7
//The States: 
8
#define MOVING 12           //Move strait.
9
#define BACKWARDS 15        //Move backwards. (Front close to wall.)
10
#define STOP 16             //Stop.  The default state, (Something broke).
11
#define CRAZY 40            //Erratic behavior that occurs more often when the robot is frequently trying to turn. (i.e. may be stuck.)
12

  
13
#define LEFT 37             //Left
14
#define RIGHT 39            //Right
15

  
16
#define BACKUP_MAX 15
17
#define CRAZY_MAX 200       //The number of counts between "crazy moments"
18
#define STRAIT_SPEED 200    //The speed when going strait or backing up.
19
#define TURN_CONSTANT 2
20
#define PCONTROL_CRAZY_LIMIT 80
21

  
22
int avoid_state;    /*State machine variable.*/
23
int crazy_count;    /*Counter for a 'get unstuck' behavior.*/
24

  
25
int backup_count;	/*Counter for backup duration.*/
26
int pControl;		/*Proportional control variable, determines turn direction.*/
27
int d1,d2,d3,d4,d5;	/*The five distances taken in by IR.*/
28

  
29
void run_around_init(void);
30
void run_around_FSM(void);
31
void evaluate_state(void);
32

  
33
#endif
trunk/code/projects/slam/Makefile
1
########Update This Section########
2
#
3
#
4

  
5
# Relative path to the root directory (containing lib directory)
6
ifndef COLONYROOT
7
COLONYROOT = ../../..
8
endif
9

  
10
# Target file name (without extension).
11
TARGET = robot_main 
12

  
13
# Uncomment this to use the wireless library
14
USE_WIRELESS = 1
15

  
16
# com1 = serial port. Use lpt1 to connect to parallel port.
17
AVRDUDE_PORT = com4
18

  
19
robot: all
20
	TARGET = robot_main
21
	
22
#
23
#
24
###################################
25

  
26
# Hey Emacs, this is a -*- makefile -*-
27
#----------------------------------------------------------------------------
28
# WinAVR Makefile Template written by Eric B. Weddington, J?rg Wunsch, et al.
29
#
30
# Released to the Public Domain
31
#
32
# Additional material for this makefile was written by:
33
# Peter Fleury
34
# Tim Henigan
35
# Colin O'Flynn
36
# Reiner Patommel
37
# Markus Pfaff
38
# Sander Pool
39
# Frederik Rouleau
40
#
41
#----------------------------------------------------------------------------
42
# On command line:
43
#
44
# make all = Make software.
45
#
46
# make clean = Clean out built project files.
47
#
48
# make coff = Convert ELF to AVR COFF.
49
#
50
# make extcoff = Convert ELF to AVR Extended COFF.
51
#
52
# make program = Download the hex file to the device, using avrdude.
53
#                Please customize the avrdude settings below first!
54
#
55
# make debug = Start either simulavr or avarice as specified for debugging, 
56
#              with avr-gdb or avr-insight as the front end for debugging.
57
#
58
# make filename.s = Just compile filename.c into the assembler code only.
59
#
60
# make filename.i = Create a preprocessed source file for use in submitting
61
#                   bug reports to the GCC project.
62
#
63
# To rebuild project do "make clean" then "make all".
64
#----------------------------------------------------------------------------
65

  
66
#if you want your code to work on the Firefly++ and not Firefly+
67
#then add the -DFFPP line to CDEFS
68
CDEFS = 
69
#-DFFPP
70

  
71
# MCU name
72
MCU = atmega128
73

  
74
# Processor frequency.
75
#     This will define a symbol, F_CPU, in all source code files equal to the 
76
#     processor frequency. You can then use this symbol in your source code to 
77
#     calculate timings. Do NOT tack on a 'UL' at the end, this will be done
78
#     automatically to create a 32-bit value in your source code.
79
F_CPU = 8000000
80

  
81
# Output format. (can be srec, ihex, binary)
82
FORMAT = ihex
83

  
84
# List C source files here. (C dependencies are automatically generated.)
85
SRC = $(wildcard *.c)
86

  
87
# List Assembler source files here.
88
#     Make them always end in a capital .S.  Files ending in a lowercase .s
89
#     will not be considered source files but generated files (assembler
90
#     output from the compiler), and will be deleted upon "make clean"!
91
#     Even though the DOS/Win* filesystem matches both .s and .S the same,
92
#     it will preserve the spelling of the filenames, and gcc itself does
93
#     care about how the name is spelled on its command-line.
94
ASRC = 
95

  
96
# Optimization level, can be [0, 1, 2, 3, s]. 
97
#     0 = turn off optimization. s = optimize for size.
98
#     (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
99
OPT = s
100

  
101
# Debugging format.
102
#     Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs.
103
#     AVR Studio 4.10 requires dwarf-2.
104
#     AVR [Extended] COFF format requires stabs, plus an avr-objcopy run.
105
DEBUG =
106

  
107
# Compiler flag to set the C Standard level.
108
#     c89   = "ANSI" C
109
#     gnu89 = c89 plus GCC extensions
110
#     c99   = ISO C99 standard (not yet fully implemented)
111
#     gnu99 = c99 plus GCC extensions
112
CSTANDARD = -std=gnu99
113

  
114
# Place -D or -U options here
115
CDEFS += -DF_CPU=$(F_CPU)UL 
116
CDEFS += -DFFP
117
# for wireless library
118
ifdef USE_WIRELESS
119
	CDEFS += -DROBOT
120
endif
121

  
122
# Place -I, -L options here
123
CINCS = -I$(COLONYROOT)/code/lib/include/libdragonfly 
124
CINCS += -L$(COLONYROOT)/code/lib/bin
125
ifdef USE_WIRELESS
126
	CINCS += -I$(COLONYROOT)/code/lib/include/libwireless
127
endif
128

  
129
#---------------- Compiler Options ----------------
130
#  -g*:          generate debugging information
131
#  -O*:          optimization level
132
#  -f...:        tuning, see GCC manual and avr-libc documentation
133
#  -Wall...:     warning level
134
#  -Wa,...:      tell GCC to pass this to the assembler.
135
#    -adhlns...: create assembler listing
136
CFLAGS =
137
# CFLAGS = -g$(DEBUG)
138
CFLAGS += $(CDEFS) $(CINCS)
139
CFLAGS += -O$(OPT)
140
CFLAGS += -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums
141
CFLAGS += -Wall -Wstrict-prototypes
142
CFLAGS += -Wa,-adhlns=$(<:.c=.lst)
143
CFLAGS += $(CSTANDARD)
144

  
145
#---------------- Assembler Options ----------------
146
#  -Wa,...:   tell GCC to pass this to the assembler.
147
#  -ahlms:    create listing
148
#  -gstabs:   have the assembler create line number information; note that
149
#             for use in COFF files, additional information about filenames
150
#             and function names needs to be present in the assembler source
151
#             files -- see avr-libc docs [FIXME: not yet described there]
152
ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs 
153

  
154

  
155
#---------------- Library Options ----------------
156
# Minimalistic printf version
157
PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min
158

  
159
# Floating point printf version (requires MATH_LIB = -lm below)
160
PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt
161

  
162
# If this is left blank, then it will use the Standard printf version.
163
PRINTF_LIB = 
164
#PRINTF_LIB = $(PRINTF_LIB_MIN)
165
#PRINTF_LIB = $(PRINTF_LIB_FLOAT)
166

  
167

  
168
# Minimalistic scanf version
169
SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min
170

  
171
# Floating point + %[ scanf version (requires MATH_LIB = -lm below)
172
SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt
173

  
174
# If this is left blank, then it will use the Standard scanf version.
175
SCANF_LIB = 
176
#SCANF_LIB = $(SCANF_LIB_MIN)
177
#SCANF_LIB = $(SCANF_LIB_FLOAT)
178

  
179
MATH_LIB = -lm
180

  
181
#---------------- External Memory Options ----------------
182

  
183
# 64 KB of external RAM, starting after internal RAM (ATmega128!),
184
# used for variables (.data/.bss) and heap (malloc()).
185
#EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff
186

  
187
# 64 KB of external RAM, starting after internal RAM (ATmega128!),
188
# only used for heap (malloc()).
189
#EXTMEMOPTS = -Wl,--defsym=__heap_start=0x801100,--defsym=__heap_end=0x80ffff
190

  
191
EXTMEMOPTS =
192

  
193
#---------------- Linker Options ----------------
194
#  -Wl,...:     tell GCC to pass this to linker.
195
#    -Map:      create map file
196
#    --cref:    add cross reference to  map file
197
LDFLAGS = -Wl,-Map=$(TARGET).map,--cref
198
LDFLAGS += $(EXTMEMOPTS)
199
LDFLAGS += $(PRINTF_LIB) $(SCANF_LIB) $(MATH_LIB)
200
ifdef USE_WIRELESS
201
	LDFLAGS += -lwireless
202
endif
203
LDFLAGS += -ldragonfly
204

  
205

  
206

  
207
#---------------- Programming Options (avrdude) ----------------
208

  
209
# Programming hardware: alf avr910 avrisp bascom bsd 
210
# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500
211
#
212
# Type: avrdude -c ?
213
# to get a full listing.
214
#
215
AVRDUDE_PROGRAMMER = avrisp
216

  
217
# programmer connected to serial device
218

  
219
AVRDUDE_WRITE_FLASH = -b 57600 -U flash:w:$(TARGET).hex
220
#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep
221

  
222

  
223
# Uncomment the following if you want avrdude's erase cycle counter.
224
# Note that this counter needs to be initialized first using -Yn,
225
# see avrdude manual.
226
#AVRDUDE_ERASE_COUNTER = -y
227

  
228
# Uncomment the following if you do /not/ wish a verification to be
229
# performed after programming the device.
230
#AVRDUDE_NO_VERIFY = -V
231

  
232
# Increase verbosity level.  Please use this when submitting bug
233
# reports about avrdude. See <http://savannah.nongnu.org/projects/avrdude> 
234
# to submit bug reports.
235
#AVRDUDE_VERBOSE = -v -v
236

  
237
AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER)
238
AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY)
239
AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE)
240
AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER)
241

  
242
#don't check for device signature
243
AVRDUDE_FLAGS += -F
244

  
245

  
246

  
247
#---------------- Debugging Options ----------------
248

  
249
# For simulavr only - target MCU frequency.
250
DEBUG_MFREQ = $(F_CPU)
251

  
252
# Set the DEBUG_UI to either gdb or insight.
253
# DEBUG_UI = gdb
254
DEBUG_UI = insight
255

  
256
# Set the debugging back-end to either avarice, simulavr.
257
DEBUG_BACKEND = avarice
258
#DEBUG_BACKEND = simulavr
259

  
260
# GDB Init Filename.
261
GDBINIT_FILE = __avr_gdbinit
262

  
263
# When using avarice settings for the JTAG
264
JTAG_DEV = /dev/com1
265

  
266
# Debugging port used to communicate between GDB / avarice / simulavr.
267
DEBUG_PORT = 4242
268

  
269
# Debugging host used to communicate between GDB / avarice / simulavr, normally
270
#     just set to localhost unless doing some sort of crazy debugging when 
271
#     avarice is running on a different computer.
272
DEBUG_HOST = localhost
273

  
274

  
275

  
276
#============================================================================
277

  
278

  
279
# Define programs and commands.
280
SHELL = sh
281
CC = avr-gcc
282
OBJCOPY = avr-objcopy
283
OBJDUMP = avr-objdump
284
SIZE = avr-size
285
NM = avr-nm
286
AVRDUDE = avrdude
287
REMOVE = rm -f
288
REMOVEDIR = rm -rf
289
COPY = cp
290
WINSHELL = cmd
291

  
292

  
293
# Define Messages
294
# English
295
MSG_ERRORS_NONE = Errors: none
296
MSG_BEGIN = -------- begin --------
297
MSG_END = --------  end  --------
298
MSG_SIZE_BEFORE = Size before: 
299
MSG_SIZE_AFTER = Size after:
300
MSG_COFF = Converting to AVR COFF:
301
MSG_EXTENDED_COFF = Converting to AVR Extended COFF:
302
MSG_FLASH = Creating load file for Flash:
303
MSG_EEPROM = Creating load file for EEPROM:
304
MSG_EXTENDED_LISTING = Creating Extended Listing:
305
MSG_SYMBOL_TABLE = Creating Symbol Table:
306
MSG_LINKING = Linking:
307
MSG_COMPILING = Compiling:
308
MSG_ASSEMBLING = Assembling:
309
MSG_CLEANING = Cleaning project:
310

  
311

  
312

  
313

  
314
# Define all object files.
315
OBJ = $(SRC:.c=.o) $(ASRC:.S=.o) 
316

  
317
# Define all listing files.
318
LST = $(SRC:.c=.lst) $(ASRC:.S=.lst) 
319

  
320

  
321
# Compiler flags to generate dependency files.
322
GENDEPFLAGS = -MD -MP -MF .dep/$(@F).d
323

  
324

  
325
# Combine all necessary flags and optional flags.
326
# Add target processor to flags.
327
ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS)
328
ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS)
329

  
330

  
331

  
332

  
333

  
334
# Default target.
335
all: begin gccversion sizebefore build sizeafter end
336

  
337
build: elf hex eep lss sym
338

  
339
elf: $(TARGET).elf
340
hex: $(TARGET).hex
341
eep: $(TARGET).eep
342
lss: $(TARGET).lss 
343
sym: $(TARGET).sym
344

  
345

  
346

  
347
# Eye candy.
348
# AVR Studio 3.x does not check make's exit code but relies on
349
# the following magic strings to be generated by the compile job.
350
begin:
351
	@echo
352
	@echo $(MSG_BEGIN)
353

  
354
end:
355
	@echo $(MSG_END)
356
	@echo
357

  
358

  
359
# Display size of file.
360
HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex
361
ELFSIZE = $(SIZE) -A $(TARGET).elf
362
AVRMEM = avr-mem.sh $(TARGET).elf $(MCU)
363

  
364
sizebefore:
365
	@if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); \
366
	$(AVRMEM) 2>/dev/null; echo; fi
367

  
368
sizeafter:
369
	@if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); \
370
	$(AVRMEM) 2>/dev/null; echo; fi
371

  
372

  
373

  
374
# Display compiler version information.
375
gccversion : 
376
	@$(CC) --version
377

  
378

  
379

  
380
# Program the device.  
381
program: $(TARGET).hex $(TARGET).eep
382
	$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)
383

  
384

  
385
# Generate avr-gdb config/init file which does the following:
386
#     define the reset signal, load the target file, connect to target, and set 
387
#     a breakpoint at main().
388
gdb-config: 
389
	@$(REMOVE) $(GDBINIT_FILE)
390
	@echo define reset >> $(GDBINIT_FILE)
391
	@echo SIGNAL SIGHUP >> $(GDBINIT_FILE)
392
	@echo end >> $(GDBINIT_FILE)
393
	@echo file $(TARGET).elf >> $(GDBINIT_FILE)
394
	@echo target remote $(DEBUG_HOST):$(DEBUG_PORT)  >> $(GDBINIT_FILE)
395
ifeq ($(DEBUG_BACKEND),simulavr)
396
	@echo load  >> $(GDBINIT_FILE)
397
endif	
398
	@echo break main >> $(GDBINIT_FILE)
399
	
400
debug: gdb-config $(TARGET).elf
401
ifeq ($(DEBUG_BACKEND), avarice)
402
	@echo Starting AVaRICE - Press enter when "waiting to connect" message displays.
403
	@$(WINSHELL) /c start avarice --jtag $(JTAG_DEV) --erase --program --file \
404
	$(TARGET).elf $(DEBUG_HOST):$(DEBUG_PORT)
405
	@$(WINSHELL) /c pause
406
	
407
else
408
	@$(WINSHELL) /c start simulavr --gdbserver --device $(MCU) --clock-freq \
409
	$(DEBUG_MFREQ) --port $(DEBUG_PORT)
410
endif
411
	@$(WINSHELL) /c start avr-$(DEBUG_UI) --command=$(GDBINIT_FILE)
412
	
413

  
414

  
415

  
416
# Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB.
417
COFFCONVERT=$(OBJCOPY) --debugging \
418
--change-section-address .data-0x800000 \
419
--change-section-address .bss-0x800000 \
420
--change-section-address .noinit-0x800000 \
421
--change-section-address .eeprom-0x810000 
422

  
423

  
424
coff: $(TARGET).elf
425
	@echo
426
	@echo $(MSG_COFF) $(TARGET).cof
427
	$(COFFCONVERT) -O coff-avr $< $(TARGET).cof
428

  
429

  
430
extcoff: $(TARGET).elf
431
	@echo
432
	@echo $(MSG_EXTENDED_COFF) $(TARGET).cof
433
	$(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof
434

  
435

  
436

  
437
# Create final output files (.hex, .eep) from ELF output file.
438
%.hex: %.elf
439
	@echo
440
	@echo $(MSG_FLASH) $@
441
	$(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@
442

  
443
%.eep: %.elf
444
	@echo
445
	@echo $(MSG_EEPROM) $@
446
	-$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \
447
	--change-section-lma .eeprom=0 -O $(FORMAT) $< $@
448

  
449
# Create extended listing file from ELF output file.
450
%.lss: %.elf
451
	@echo
452
	@echo $(MSG_EXTENDED_LISTING) $@
453
	$(OBJDUMP) -h -S $< > $@
454

  
455
# Create a symbol table from ELF output file.
456
%.sym: %.elf
457
	@echo
458
	@echo $(MSG_SYMBOL_TABLE) $@
459
	$(NM) -n $< > $@
460

  
461

  
462

  
463
# Link: create ELF output file from object files.
464
.SECONDARY : $(TARGET).elf
465
.PRECIOUS : $(OBJ)
466
%.elf: $(OBJ)
467
	@echo
468
	@echo $(MSG_LINKING) $@
469
	$(CC) $(ALL_CFLAGS) $^ --output $@ $(LDFLAGS)
470

  
471

  
472
# Compile: create object files from C source files.
473
%.o : %.c
474
	@echo
475
	@echo $(MSG_COMPILING) $<
476
	$(CC) -c $(ALL_CFLAGS) $< -o $@ 
477

  
478

  
479
# Compile: create assembler files from C source files.
480
%.s : %.c
481
	$(CC) -S $(ALL_CFLAGS) $< -o $@
482

  
483

  
484
# Assemble: create object files from assembler source files.
485
%.o : %.S
486
	@echo
487
	@echo $(MSG_ASSEMBLING) $<
488
	$(CC) -c $(ALL_ASFLAGS) $< -o $@
489

  
490
# Create preprocessed source for use in sending a bug report.
491
%.i : %.c
492
	$(CC) -E -mmcu=$(MCU) -I. $(CFLAGS) $< -o $@ 
493

  
494

  
495
# Target: clean project.
496
clean: begin clean_list end
497

  
498
clean_list :
499
	@echo
500
	@echo $(MSG_CLEANING)
501
	$(REMOVE) $(TARGET).hex
502
	$(REMOVE) $(TARGET).eep
503
	$(REMOVE) $(TARGET).cof
504
	$(REMOVE) $(TARGET).elf
505
	$(REMOVE) $(TARGET).map
506
	$(REMOVE) $(TARGET).sym
507
	$(REMOVE) $(TARGET).lss
508
	$(REMOVE) $(OBJ)
509
	$(REMOVE) $(LST)
510
	$(REMOVE) $(SRC:.c=.s)
511
	$(REMOVE) $(SRC:.c=.d)
512
	$(REMOVEDIR) .dep
513

  
514

  
515

  
516
# Include the dependency files.
517
-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*)
518

  
519

  
520
# Listing of phony targets.
521
.PHONY : all begin finish end sizebefore sizeafter gccversion \
522
build elf hex eep lss sym coff extcoff \
523
clean clean_list program debug gdb-config
524

  
trunk/code/projects/slam.bak2/computer/wl_defs.h
1
/**
2
 * Copyright (c) 2007 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

  
26
/**
27
 * @file wl_defs.h
28
 * @brief Definitions for Wireless
29
 *
30
 * Contains definitions for wireless packet groups, packet types,
31
 * debugging information, etc.
32
 *
33
 * @author Brian Coltin, Colony Project, CMU Robotics Club
34
 **/
35

  
36
#ifndef WL_DEFS_H
37
#define WL_DEFS_H
38

  
39
//comment out this line if using a computer hooked up to an xbee
40
//#define ROBOT
41

  
42
//uncomment this line for debug information
43
//#define WL_DEBUG
44

  
45
// Packet Groups and Types
46

  
47
// Error group
48
#define WL_ERROR_GROUP 1
49

  
50
#define WL_ERROR_STRING_TYPE 1
51

  
52
// Token Ring group
53
#define WL_TOKEN_RING_GROUP 2
54

  
55
#define WL_TOKEN_PASS 1
56
#define WL_TOKEN_BOM_ON 2
57
#define WL_TOKEN_JOIN 3
58
#define WL_TOKEN_JOIN_ACCEPT 4
59

  
60
// timing constants
61
#ifndef FIREFLY
62
#define BOM_DELAY 50
63
#else
64
#define BOM_DELAY 100
65
#endif
66

  
67
#define DEATH_DELAY 4
68
#define JOIN_DELAY 8
69

  
70
// Recharging group
71
#define WL_RECHARGE_GROUP 3
72

  
73
#define WL_RECHARGE_POLL_STATIONS 1
74
#define WL_RECHARGE_STATION_AVAILABLE 2
75
#define WL_RECHARGE_REQUEST 3
76
#define WL_RECHARGE_REQUEST_ACCEPT 4
77
#define WL_RECHARGE_VERIFY 5
78
#define WL_RECHARGE_CANCEL 6
79
#define WL_RECHARGE_SEEKING 7
80
#define WL_RECHARGE_DOCKED 8
81

  
82
#ifdef WL_DEBUG
83

  
84
#ifdef ROBOT
85
#include <serial.h>
86
#endif
87

  
88
#ifdef ROBOT
89
#define WL_DEBUG_PRINT( s ) usb_puts( s )
90
#else
91
#define WL_DEBUG_PRINT( s ) printf( s )
92
#endif
93

  
94
#ifdef ROBOT
95
#define WL_DEBUG_PRINT_INT( i ) usb_puti(i)
96
#else
97
#define WL_DEBUG_PRINT_INT( i ) printf("%i", i)
98
#endif
99

  
100
#else
101

  
102
#define WL_DEBUG_PRINT( s )
103
#define WL_DEBUG_PRINT_INT( i )
104

  
105
#endif
106

  
107
#endif
108

  
trunk/code/projects/slam.bak2/computer/data_requests.h
1
#define DATA_REQUEST_GROUP 7
2
#define BOM_TYPE 0
3
#define IR_TYPE 1
4
#define ENCODER_TYPE 2
5
#define ALL_TYPE 3
6

  
7
/** 
8
* @brief I tried to leave the data in an easy manipulable form, however
9
* the side effect is that you're going to have to reference here sometimes.
10
* The fields are obvious...
11
*   robot_id is the id of the robot that was being looked at by robot that the
12
* list encapsulates.
13
*   value is the 0-15 value of the IR receiver that saw the robot. (Counter clockwise
14
* from the right side.
15
*   next is a pointer to the next BomNode (or NULL).
16
*/
17
typedef struct {
18
  short int robot_id;
19
  char value;
20
  struct BomNode* next;
21
} BomNode;
22

  
23
/*typedef struct{
24
    unsigned char* packet;
25
    char type;
26
    int source;
27
    int length;
28
} RawData;*/
29

  
30
/** 
31
* @brief Initializes the Data Request functionality on the server side.
32
* 
33
* @param void*(bom_data_handler(BomNode** head)) A function that accepts 
34
* a BomNode** as an argument and handles the bom data however you would like.
35
* 
36
* @param void*(IR_data_handler(short** data)) A function that accepts an
37
* array of shorts for processing.
38
*
39
* @param void*(encoder_data_handler(unsigned char** data)) A function that
40
* accepts an array of unsigned chars (tentatively) as data for processing.
41
*/
42
void data_requests_init(
43
                        void(*bom_data_handler)(short id, BomNode** head),
44
                        void(*IR_data_handler)(short id, short** data),
45
                        void(*encoder_data_handler)(short id, unsigned char** data));
46

  
47
/** 
48
* @brief Pings the robot for BOM data.
49
* 
50
* @param robot_id The robot ID of the robot to ping. Typically you will need
51
* to join (if not insert yourself into) the token ring to get a list of robot
52
* id's.
53
*/
54
void request_bom_data(int robot_id);
55

  
56
/** 
57
* @brief Pings the robot for BOM data.
58
*/
59
void request_IR_data(int robot_id);
60

  
61
/** 
62
* @brief Pings the robot for encoder data.
63
* NOT YET DONE (We don't have encoders yet...)
64
*/
65
void request_encoder_data(int robot_id);
66

  
67
void request_all(int robot_id);
trunk/code/projects/slam.bak2/computer/slam.txt
1
This is the game plan if anyone is interested.
2
Each robot will go for an unspecified amount of time,
3
accruing encoder, BOM, and IR data.  The IR and encoders
4
will be used to generate heat maps for each robot, starting
5
at the center, decreasing in certainty (i.e. the amount of
6
'color' put on during a time unit will be spread over a greater
7
area) over time according to the physical limitations of 
8
the measuring devices.
9

  
10
When the uncertainty gets too high, a new map will be allocated for
11
the robot.
12

  
13
The BOM data will be used to orient individual maps to one another.
14
Along with physical limitations on the size of the environment, and
15
hopefully some brainstorming to do with triangularization and what not,
16
the heat maps will become increasingly well oriented.
17

  
18
Finally, after a currently unspecified amount of time, the maps which have
19
been properly oriented (this could probably just be all of them due to the nature
20
of the heat maps,) will be summed up into one meta map! Metamap will undergo some
21
signal processing, probably in the form of a second order gradient, to get a map
22
of magnitudes, which we will try to further reduce to lines.  These lines are the
23
walls of the environment.
24

  
25
In time, it is possible that this "unspecfied amount of time" could become a continuous
26
computation, but I haven't put much thought into how this will scale.  It will almost 
27
definitely depend upon the chosen resolution of the heat map.
28

  
29
I do want people working with me, this should be a lot of fun,
30
I think this is a solid method of mapping.
trunk/code/projects/slam.bak2/computer/queue.h
1
/**
2
 * Copyright (c) 2007 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

  
26
/**
27
 * @file queue.h
28
 * @brief A queue implementation
29
 * 
30
 * Implements a queue, a first in, first out data structure.
31
 *
32
 * @author Brian Coltin, Colony Project, CMU Robotics Club
33
 **/
34

  
35
#ifndef WIRELESS_QUEUE_H
36
#define WIRELESS_QUEUE_H
37

  
38
struct node_def;
39

  
40
/**
41
 * @defgroup queue Queue
42
 * @brief A queue implementation
43
 * 
44
 * A queue implementation.
45
 *
46
 * @{
47
 **/
48

  
49
/**
50
 * @struct Queue
51
 * Represents a queue, a first in, first out data structure.
52
 **/
53
typedef struct
54
{
55
	/**
56
	 * The head of the queue, the next item to be removed.
57
	 **/
58
	struct node_def* head;
59
	/**
60
	 * The tail of the queue, the last item added.
61
	 **/
62
	struct node_def* tail;
63
	/**
64
	 * The number of elements in the queue.
65
	 **/
66
	int size;
67
} Queue;
68

  
69
/** @brief Create a new queue **/
70
Queue* queue_create(void);
71
/** @brief Destroy a queue **/
72
void queue_destroy(Queue* q);
73
/** @brief Add an element to a queue **/
74
void queue_add(Queue* q, void* item);
75
/** @brief Remove an element from a queue **/
76
void* queue_remove(Queue* q);
77
/** @brief Remove all instances of a given element from a queue **/
78
void queue_remove_all(Queue* q, void* item);
79
/** @brief Get the size of a queue **/
80
int queue_size(Queue* q);
81
/** @brief Check if the queue is empty **/
82
int queue_is_empty(Queue* q);
83

  
84
/** @} **/
85

  
86

  
87
#endif
88

  
trunk/code/projects/slam.bak2/computer/server_main.c
1
#include <stdio.h>
2
#include <stdlib.h>
3
#include <sys/time.h>
4
#include "data_requests.h"
5
#include "queue.h"
6
#include "math.h"
7

  
8
#define DEFAULT_WIDTH 512
9
#define DEFAULT_HEIGHT 512
10
#define MAX_MAPS 30
11

  
12
#define MAX_ID 20
13

  
14
typedef struct dataNode{
15
  struct timeval* timeStamp;
16
  char type;
17
  union{
18
      BomNode* u_b;
19
      short* u_s;
20
      unsigned char* u_c;
21
  } data;
22
  short robot_id;
23
} Data;
24

  
25
/** 
26
* Map oriented data.
27
*/
28

  
29
typedef struct Map{
30
  int data[DEFAULT_WIDTH][DEFAULT_HEIGHT];
31
  int robot_x;
32
  int robot_y;
33
  double robot_orientation;
34
  short robot_id;
35
  char correlation_group;  
36
} Map;
37

  
38
Map** map_array;
39
int map_count;
40

  
41
/**Robot oriented data.**/
42
Queue* dataQueue;
43
int robot_map[MAX_ID];
44

  
45
/*These functions simply add the data to the queue.*/
46
void bom_handler(short id, BomNode** head);
47
void IR_handler(short id, short** data);
48
void encoder_handler(short id, unsigned char** data);
49

  
50
void slam_analyze(void);
51
void bom_analyze(BomNode* head,short robot_id);
52
void IR_analyze(short* data, short robot_id);
53
void encoder_analyze(unsigned char* data, short robot_id);
54

  
55
void initialize(void);
56

  
57
int main(void){
58
  data_requests_init(bom_handler,IR_handler,encoder_handler);
59
  dataQueue = queue_create();
60
  initialize();
61
  slam_analyze(); 
62
  return -1;
63
}
64

  
65
void initialize(void){
66
  map_array = malloc(MAX_MAPS * sizeof(Map*));
67
  map_count = 0;
68
  int i;
69
  for(i=0; i<MAX_ID;i++){
70
    robot_map[i] = -1;
71
  }
72
}
73

  
74
void slam_analyze(void){
75
  Data* current_data;
76
  while(1){
77
    if(!queue_is_empty(dataQueue)){
78
      if(queue_size(dataQueue) < 5){
79
        //request_all(/*The robot that most needs it*/);
80
      }
81
      current_data = (Data*)queue_remove(dataQueue);
82
      switch(current_data->type){
83
      
84
      case(BOM_TYPE):
85
        bom_analyze(current_data->data.u_b,current_data->robot_id);
86
        break;
87
      
88
      case(IR_TYPE):
89
        IR_analyze(current_data->data.u_s,current_data->robot_id);
90
        break;
91

  
92
      case(ENCODER_TYPE):
93
        encoder_analyze(current_data->data.u_c,current_data->robot_id);
94
        break;
95
      }
96
    }
97
  }
98
}
99

  
100
void initialize_map(int robot_id){
101
    int i,j;
102
    map_array[map_count] = (Map*)malloc(sizeof(Map));
103
    for(i=0;i<DEFAULT_WIDTH;i++){
104
        for(j=0;j<DEFAULT_HEIGHT;j++){
105
          map_array[map_count]->data[i][j] = 0;
106
        }
107
    }
108
    map_array[map_count]->robot_x = DEFAULT_WIDTH / 2;
109
    map_array[map_count]->robot_y = DEFAULT_HEIGHT/ 2;
110
    map_array[map_count]->robot_orientation = 0;
111
    map_array[map_count]->robot_id = robot_id;
112
    map_array[map_count]->correlation_group = -1;
113
    robot_map[robot_id] = map_count++;
114
}
115

  
116
void bom_analyze(BomNode* head, short robot_id){
117
  BomNode* current_node = head;
118
  if(robot_map[robot_id]==-1) initialize_map(robot_id);
119
      
120
  while(current_node!=NULL){
121
    
122
    current_node = current_node->next;
123
  }
124
}
125

  
126
void IR_analyze(short* data, short robot_id){
127
  if(robot_map[robot_id]==-1) initialize_map(robot_id);
128

  
129
}
130

  
131
void encoder_analyze(unsigned char* data, short robot_id){
132
  if(robot_map[robot_id]==-1) initialize_map(robot_id);
133

  
134
}
135

  
136
/*The packet handling functions for nata requests.*/
137

  
138
void bom_handler(short id, BomNode** head){
139
  Data* newData = malloc(sizeof(Data));
140
  newData->type = BOM_TYPE;
141
  newData->data.u_b = (*head);
142
  newData->robot_id = id;
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff