root / trunk / code / lib / src / libwireless / sensor_matrix.c @ 48
History | View | Annotate | Download (5.82 KB)
1 | 18 | bcoltin | #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 = 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 = malloc(m->size * sizeof(int*)); |
||
30 | m->joined = 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] = 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 = 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] = 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 = 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 | } |