root / branches / autonomous_recharging / code / projects / autonomous_recharging / charging_station / sensor_matrix.c @ 198
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 |
|