Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / libwireless / lib / sensor_matrix.c @ 162

History | View | Annotate | Download (5.87 KB)

1
#include <stdlib.h>
2
#include <stdio.h>
3
#include <wl_defs.h>
4

    
5
#include "sensor_matrix.h"
6

    
7
#define DEFAULT_SENSOR_MATRIX_SIZE 20
8

    
9
/*Sensor Matrix Functions*/
10
void sensor_matrix_expand(SensorMatrix* m, int nextSize);
11

    
12
/**
13
 * Initializes the sensor matrix.
14
 *
15
 * @return the newly created sensor matrix
16
 **/
17
SensorMatrix* sensor_matrix_create()
18
{
19
        SensorMatrix* m;
20
        int i;
21
        
22
        m = (SensorMatrix*)malloc(sizeof(SensorMatrix));
23
        if (!m)
24
        {
25
                WL_DEBUG_PRINT("Out of memory - create sensor matrix.\r\n");
26
                return NULL;
27
        }
28
        m->size = DEFAULT_SENSOR_MATRIX_SIZE;
29
        m->matrix = (int**)malloc(m->size * sizeof(int*));
30
        m->joined = (int*)malloc(m->size * sizeof(int));
31
        m->numJoined = 0;
32
        if (!(m->matrix) || !(m->joined))
33
        {
34
                WL_DEBUG_PRINT("Out of memory - create sensor matrix 2.\r\n");
35
                return NULL;
36
        }
37
        
38
        for (i = 0; i < m->size; i++)
39
        {
40
                m->matrix[i] = NULL;
41
                m->joined[i] = 0;
42
        }
43
        return m;
44
}
45

    
46
/**
47
 * Deletes and frees memory from the sensor matrix.
48
 *
49
 * @param m the sensor matrix to delete
50
 **/
51
void sensor_matrix_destroy(SensorMatrix* m)
52
{
53
        int i;
54
        for (i = 0; i < m->size; i++)
55
                if (m->matrix[i] != NULL)
56
                        free(m->matrix[i]);
57
        free(m->matrix);
58
        free(m->joined);
59
        free(m);
60
}
61

    
62
/**
63
 * Adds robot with XBee id id to the sensor matrix.
64
 * 
65
 * @param m the sensor matrix
66
 * @param id the XBee ID of the robot to add
67
 **/
68
void sensor_matrix_add_robot(SensorMatrix* m, unsigned int id)
69
{
70
        int i;
71
        if (id >= m->size)
72
                sensor_matrix_expand(m, id + 1);
73
        if (m->matrix[id] != NULL)
74
                return;
75
        
76
        m->matrix[id] = (int*)malloc(m->size * sizeof(int));
77
        if (!(m->matrix[id]))
78
        {
79
                WL_DEBUG_PRINT("Out of memory - add robot.\r\n");
80
                return;
81
        }
82

    
83
        for (i = 0; i < m->size; i++)
84
                if (m->matrix[i] != NULL)
85
                        m->matrix[i][id] = -1;
86
}
87

    
88
/**
89
 * Removes robot with id from the sensor matrix, and removes
90
 * all sensor information regarding the robot.
91
 *
92
 * @param m the sensor matrix
93
 * @param id the XBee ID of the robot to remove
94
 **/
95
void sensor_matrix_remove_robot(SensorMatrix* m, unsigned int id)
96
{
97
        int i;
98

    
99
        if (id >= m->size || m->matrix[id] == NULL)
100
        {
101
                WL_DEBUG_PRINT("Removing robot not added to matrix.\r\n");
102
                return;
103
        }
104
        
105
        free(m->matrix[id]);
106
        m->matrix[id] = NULL;
107
        
108
        for (i = 0 ; i < m->size; i++)
109
                if (m->matrix[i] != NULL)
110
                        m->matrix[i][id] = -1;
111
        
112
        m->joined[id] = 0;
113
}
114

    
115
/**
116
 * Expands the size of the sensor matrix if an id number we attempt
117
 * to add is too large.
118
 *
119
 * @param m the sensor matrix to expand
120
 * @param size the new size of the sensor matrix
121
 **/
122
//Note: this has probably not been tested, hopefully it works
123
void sensor_matrix_expand(SensorMatrix* m, int nextSize)
124
{
125
        int i, j;
126
        WL_DEBUG_PRINT("Expanding sensor matrix.\r\n");
127
        
128
        int** tempMatrix = (int**)malloc(nextSize * sizeof(int*));
129
        if (!tempMatrix)
130
        {
131
                WL_DEBUG_PRINT("Out of memory - expand matrix.\r\n");
132
                return;
133
        }
134
        
135
        for (i = 0; i < nextSize; i++)
136
                tempMatrix[i] = NULL;
137
        
138
        //copy over old sensor data
139
        for (i = 0; i < m->size; i++)
140
                if (m->matrix[i] != NULL)
141
                {
142
                  tempMatrix[i] = (int *)malloc(nextSize * sizeof(int));
143
                        if (!tempMatrix[i])
144
                        {
145
                                WL_DEBUG_PRINT("Out of memory - expand matrix 2.\r\n");
146
                                return;
147
                        }
148
                        for (j = 0; j < m->size; j++)
149
                                tempMatrix[i][j] = m->matrix[i][j];
150
                        for (j = m->size; j < nextSize; j++)
151
                                tempMatrix[i][j] = -1;
152
                        
153
                        free(m->matrix[i]);
154
                }
155
        
156
        free(m->matrix);
157
        m->matrix = tempMatrix;
158
        m->size = nextSize;
159

    
160
        //expand the size of joined
161
        int* tempJoined = (int *)malloc(nextSize * sizeof(int));
162
        if (!tempJoined)
163
        {
164
                WL_DEBUG_PRINT("Out of memory - expand matrix 3.\r\n");
165
                return;
166
        }
167
        
168
        for (i = 0; i < m->size; i++)
169
                tempJoined[i] = m->joined[i];
170
        for (i = m->size; i < nextSize; i++)
171
                tempJoined[i] = 0;
172
        
173
        free(m->joined);
174
        m->joined = tempJoined;
175
}
176

    
177
/**
178
 * Sets the sensor reading for robot robot to reading.
179
 * 
180
 * @param m the sensor matrix to set the reading for
181
 * @param observer the id of the robot who made the reading
182
 * @param robot the id of the robot who the reading is for
183
 * @param reading the BOM reading from observer to robot
184
 */
185
void sensor_matrix_set_reading(SensorMatrix* m, int observer, int robot, int reading)
186
{
187
        if (robot >= m->size || observer >= m->size || m->matrix[observer] == NULL)
188
                sensor_matrix_add_robot(m, observer);
189
        m->matrix[observer][robot] = reading;
190
}
191

    
192
/**
193
 * Gets the sensor reading for a robot to another robot.
194
 *
195
 * @param m the sensor matrix
196
 * @param observer the robot whose reading we check
197
 * @param robot the robot who we are checking the reading to
198
 *
199
 * @return the observer's BOM reading for robot
200
 **/
201
int sensor_matrix_get_reading(SensorMatrix* m, int observer, int robot)
202
{
203
        if (observer >= m->size || robot >= m->size)
204
                return -1;
205
        return m->matrix[observer][robot];
206
}
207

    
208
/**
209
 * Sets whether or not the given robot is part of the token ring.
210
 *
211
 * @param m the sensor matrix
212
 * @param robot the robot to set as a member / nonmember of the token ring
213
 * @param in 1 if the robot is in the token ring, 0 otherwise
214
 **/
215
void sensor_matrix_set_in_ring(SensorMatrix* m, int robot, int in)
216
{
217
        if (robot >= m->size)
218
                sensor_matrix_expand(m, robot + 1);
219
        if (in == 1)
220
                sensor_matrix_add_robot(m, robot);
221
        if (in == 1 && m->joined[robot] == 0)
222
                m->numJoined++;
223
        if (in == 0 && m->joined[robot] == 1)
224
                m->numJoined--;
225
        m->joined[robot] = in;
226
}
227

    
228
/**
229
 * Checks if the given robot is in the token ring.
230
 * 
231
 * @param m the sensor matrix
232
 * @param robot the ID of the robot to check
233
 *
234
 * @return 1 if the robot is in the token ring, 0 otherwise
235
 **/
236
int sensor_matrix_get_in_ring(SensorMatrix* m, int robot)
237
{
238
        if (robot >= m->size)
239
                return -1;
240
        return m->joined[robot];
241
}
242

    
243
/**
244
 * Returns the size of the sensor matrix.
245
 *
246
 * @param m the sensor matrix
247
 * 
248
 * @return the size of the sensor matrix
249
 **/
250
int sensor_matrix_get_size(SensorMatrix* m)
251
{
252
        return m->size;
253
}
254

    
255
/**
256
 * Returns the number of robots which have joined the
257
 * token ring.
258
 *
259
 * @param m the sensor matrix
260
 *
261
 * @return the number of robots in the token ring
262
 **/
263
int sensor_matrix_get_joined(SensorMatrix* m)
264
{
265
        return m->numJoined;
266
}
267