Project

General

Profile

Statistics
| Revision:

root / trunk / code / lib / src / libwireless / sensor_matrix.c @ 366

History | View | Annotate | Download (7.44 KB)

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 sensor_matrix.c
28
 * @brief Sensor Matrix implementation
29
 *
30
 * Implementation of a sensor matrix for storing localization implementation.
31
 *
32
 * @author Brian Coltin, Colony Project, CMU Robotics Club
33
 **/
34

    
35
#include <stdlib.h>
36
#include <stdio.h>
37
#include <wl_defs.h>
38

    
39
#include "sensor_matrix.h"
40

    
41
#define DEFAULT_SENSOR_MATRIX_SIZE 20
42

    
43
/*Sensor Matrix Functions*/
44
void sensor_matrix_expand(SensorMatrix* m, int nextSize);
45

    
46
/**
47
 * Initializes the sensor matrix.
48
 *
49
 * @return the newly created sensor matrix
50
 **/
51
SensorMatrix* sensor_matrix_create()
52
{
53
        SensorMatrix* m;
54
        int i;
55
        
56
        m = (SensorMatrix*)malloc(sizeof(SensorMatrix));
57
        if (!m)
58
        {
59
                WL_DEBUG_PRINT("Out of memory - create sensor matrix.\r\n");
60
                return NULL;
61
        }
62
        m->size = DEFAULT_SENSOR_MATRIX_SIZE;
63
        m->matrix = (int**)malloc(m->size * sizeof(int*));
64
        if (!(m->matrix)) {
65
          WL_DEBUG_PRINT("Out of memory - allocating memory for matrix.\r\n");
66
          free(m);
67
          return NULL;
68
        }
69
        m->joined = (int*)malloc(m->size * sizeof(int));
70
        if (!(m->joined)) {
71
          WL_DEBUG_PRINT("Out of memory - allocating memory for joined.\r\n");
72
          free(m->matrix);
73
          free(m);
74
          return NULL;
75
        }
76
        m->numJoined = 0;
77
        if (!(m->matrix) || !(m->joined))
78
        {
79
                WL_DEBUG_PRINT("Out of memory - create sensor matrix 2.\r\n");
80
                return NULL;
81
        }
82
        
83
        for (i = 0; i < m->size; i++)
84
        {
85
                m->matrix[i] = NULL;
86
                m->joined[i] = 0;
87
        }
88
        return m;
89
}
90

    
91
/**
92
 * Deletes and frees memory from the sensor matrix.
93
 *
94
 * @param m the sensor matrix to delete
95
 **/
96
void sensor_matrix_destroy(SensorMatrix* m)
97
{
98
        int i;
99
        for (i = 0; i < m->size; i++)
100
                if (m->matrix[i] != NULL)
101
                        free(m->matrix[i]);
102
        free(m->matrix);
103
        free(m->joined);
104
        free(m);
105
}
106

    
107
/**
108
 * Adds robot with XBee id id to the sensor matrix.
109
 * 
110
 * @param m the sensor matrix
111
 * @param id the XBee ID of the robot to add
112
 **/
113
void sensor_matrix_add_robot(SensorMatrix* m, int id)
114
{
115
        int i;
116
        if (id >= m->size)
117
                sensor_matrix_expand(m, id + 1);
118
        if (m->matrix[id] != NULL)
119
                return;
120
        
121
        m->matrix[id] = (int*)malloc(m->size * sizeof(int));
122
        if (!(m->matrix[id]))
123
        {
124
                WL_DEBUG_PRINT("Out of memory - add robot.\r\n");
125
                return;
126
        }
127

    
128
        for (i = 0; i < m->size; i++)
129
                if (m->matrix[i] != NULL)
130
                        m->matrix[i][id] = -1;
131
}
132

    
133
/**
134
 * Removes robot with id from the sensor matrix, and removes
135
 * all sensor information regarding the robot.
136
 *
137
 * @param m the sensor matrix
138
 * @param id the XBee ID of the robot to remove
139
 **/
140
void sensor_matrix_remove_robot(SensorMatrix* m, int id)
141
{
142
        int i;
143

    
144
        if (id >= m->size || m->matrix[id] == NULL)
145
        {
146
                WL_DEBUG_PRINT("Removing robot not added to matrix.\r\n");
147
                return;
148
        }
149
        
150
        free(m->matrix[id]);
151
        m->matrix[id] = NULL;
152
        
153
        for (i = 0 ; i < m->size; i++)
154
                if (m->matrix[i] != NULL)
155
                        m->matrix[i][id] = -1;
156
        
157
        m->joined[id] = 0;
158
}
159

    
160
/**
161
 * Expands the size of the sensor matrix if an id number we attempt
162
 * to add is too large.
163
 *
164
 * @param m the sensor matrix to expand
165
 * @param size the new size of the sensor matrix
166
 **/
167
//Note: this has probably not been tested, hopefully it works
168
void sensor_matrix_expand(SensorMatrix* m, int nextSize)
169
{
170
        int i, j;
171
        WL_DEBUG_PRINT("Expanding sensor matrix.\r\n");
172
        
173
        int** tempMatrix = (int**)malloc(nextSize * sizeof(int*));
174
        if (!tempMatrix)
175
        {
176
                WL_DEBUG_PRINT("Out of memory - expand matrix.\r\n");
177
                return;
178
        }
179
        
180
        for (i = 0; i < nextSize; i++)
181
                tempMatrix[i] = NULL;
182
        
183
        //copy over old sensor data
184
        for (i = 0; i < m->size; i++)
185
                if (m->matrix[i] != NULL)
186
                {
187
                  tempMatrix[i] = (int *)malloc(nextSize * sizeof(int));
188
                        if (!tempMatrix[i])
189
                        {
190
                                WL_DEBUG_PRINT("Out of memory - expand matrix 2.\r\n");
191
                                return;
192
                        }
193
                        for (j = 0; j < m->size; j++)
194
                                tempMatrix[i][j] = m->matrix[i][j];
195
                        for (j = m->size; j < nextSize; j++)
196
                                tempMatrix[i][j] = -1;
197
                        
198
                        free(m->matrix[i]);
199
                }
200
        
201
        free(m->matrix);
202
        m->matrix = tempMatrix;
203
        m->size = nextSize;
204

    
205
        //expand the size of joined
206
        int* tempJoined = (int *)malloc(nextSize * sizeof(int));
207
        if (!tempJoined)
208
        {
209
                WL_DEBUG_PRINT("Out of memory - expand matrix 3.\r\n");
210
                return;
211
        }
212
        
213
        for (i = 0; i < m->size; i++)
214
                tempJoined[i] = m->joined[i];
215
        for (i = m->size; i < nextSize; i++)
216
                tempJoined[i] = 0;
217
        
218
        free(m->joined);
219
        m->joined = tempJoined;
220
}
221

    
222
/**
223
 * Sets the sensor reading for robot robot to reading.
224
 * 
225
 * @param m the sensor matrix to set the reading for
226
 * @param observer the id of the robot who made the reading
227
 * @param robot the id of the robot who the reading is for
228
 * @param reading the BOM reading from observer to robot
229
 */
230
void sensor_matrix_set_reading(SensorMatrix* m, int observer, int robot, int reading)
231
{
232
        if (robot >= m->size || observer >= m->size || m->matrix[observer] == NULL)
233
                sensor_matrix_add_robot(m, observer);
234
        m->matrix[observer][robot] = reading;
235
}
236

    
237
/**
238
 * Gets the sensor reading for a robot to another robot.
239
 *
240
 * @param m the sensor matrix
241
 * @param observer the robot whose reading we check
242
 * @param robot the robot who we are checking the reading to
243
 *
244
 * @return the observer's BOM reading for robot
245
 **/
246
int sensor_matrix_get_reading(SensorMatrix* m, int observer, int robot)
247
{
248
        if (observer >= m->size || robot >= m->size)
249
                return -1;
250
        return m->matrix[observer][robot];
251
}
252

    
253
/**
254
 * Sets whether or not the given robot is part of the token ring.
255
 *
256
 * @param m the sensor matrix
257
 * @param robot the robot to set as a member / nonmember of the token ring
258
 * @param in 1 if the robot is in the token ring, 0 otherwise
259
 **/
260
void sensor_matrix_set_in_ring(SensorMatrix* m, int robot, int in)
261
{
262
        if (robot >= m->size)
263
                sensor_matrix_expand(m, robot + 1);
264
        if (in == 1)
265
                sensor_matrix_add_robot(m, robot);
266
        if (in == 1 && m->joined[robot] == 0)
267
                m->numJoined++;
268
        if (in == 0 && m->joined[robot] == 1)
269
                m->numJoined--;
270
        m->joined[robot] = in;
271
}
272

    
273
/**
274
 * Checks if the given robot is in the token ring.
275
 * 
276
 * @param m the sensor matrix
277
 * @param robot the ID of the robot to check
278
 *
279
 * @return 1 if the robot is in the token ring, 0 otherwise
280
 **/
281
int sensor_matrix_get_in_ring(SensorMatrix* m, int robot)
282
{
283
        if (robot >= m->size)
284
                return -1;
285
        return m->joined[robot];
286
}
287

    
288
/**
289
 * Returns the size of the sensor matrix.
290
 *
291
 * @param m the sensor matrix
292
 * 
293
 * @return the size of the sensor matrix
294
 **/
295
int sensor_matrix_get_size(SensorMatrix* m)
296
{
297
        return m->size;
298
}
299

    
300
/**
301
 * Returns the number of robots which have joined the
302
 * token ring.
303
 *
304
 * @param m the sensor matrix
305
 *
306
 * @return the number of robots in the token ring
307
 **/
308
int sensor_matrix_get_joined(SensorMatrix* m)
309
{
310
        return m->numJoined;
311
}
312