Project

General

Profile

Revision 1345

Added by Rich Hong over 11 years ago

Final spline code for master/slave

updated outdated libdragonfly and libwireless

View differences:

sensor_matrix.c
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 1
/**
13
 * Initializes the sensor matrix.
2
 * Copyright (c) 2007 Colony Project
14 3
 *
15
 * @return the newly created sensor matrix
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.
16 24
 **/
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 25

  
46 26
/**
47
 * Deletes and frees memory from the sensor matrix.
27
 * @file sensor_matrix.c
28
 * @brief Sensor Matrix implementation
48 29
 *
49
 * @param m the sensor matrix to delete
30
 * Implementation of a sensor matrix for storing localization implementation.
31
 *
32
 * @author Brian Coltin, Colony Project, CMU Robotics Club
50 33
 **/
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 34

  
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
	}
35
#include <stdlib.h>
36
#include <stdio.h>
37
#include <wl_defs.h>
82 38

  
83
	for (i = 0; i < m->size; i++)
84
		if (m->matrix[i] != NULL)
85
			m->matrix[i][id] = -1;
86
}
39
#include "sensor_matrix.h"
87 40

  
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;
41
// the global sensor matrix
42
SensorMatrix m;
98 43

  
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 44
/**
116
 * Expands the size of the sensor matrix if an id number we attempt
117
 * to add is too large.
45
 * Initializes the sensor matrix.
118 46
 *
119
 * @param m the sensor matrix to expand
120
 * @param size the new size of the sensor matrix
47
 * @return the newly created sensor matrix
121 48
 **/
122
//Note: this has probably not been tested, hopefully it works
123
void sensor_matrix_expand(SensorMatrix* m, int nextSize)
49
void sensor_matrix_create()
124 50
{
125 51
	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 52

  
160
	//expand the size of joined
161
	int* tempJoined = malloc(nextSize * sizeof(int));
162
	if (!tempJoined)
53
	m.numJoined = 0;
54

  
55
	for (i = 0; i < MAXIMUM_XBEE_ID; i++)
163 56
	{
164
		WL_DEBUG_PRINT("Out of memory - expand matrix 3.\r\n");
165
		return;
57
		m.joined[i] = 0;
58
#ifndef BAYBOARD
59
		for (j = 0; j < MAXIMUM_XBEE_ID; j++)
60
			m.matrix[i][j] = READING_UNKNOWN;
61
#endif
166 62
	}
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 63
}
176 64

  
177 65
/**
178 66
 * Sets the sensor reading for robot robot to reading.
179
 * 
180
 * @param m the sensor matrix to set the reading for
67
 *
181 68
 * @param observer the id of the robot who made the reading
182 69
 * @param robot the id of the robot who the reading is for
183 70
 * @param reading the BOM reading from observer to robot
184 71
 */
185
void sensor_matrix_set_reading(SensorMatrix* m, int observer, int robot, int reading)
72
void sensor_matrix_set_reading(int observer, int robot, int reading)
186 73
{
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;
74
#ifndef BAYBOARD
75
	if (robot >= MAXIMUM_XBEE_ID || observer >= MAXIMUM_XBEE_ID)
76
	{
77
		WL_DEBUG_PRINT("ID too large.");
78
		return;
79
	}
80

  
81
	m.matrix[observer][robot] = (unsigned char)reading;
82
#endif
190 83
}
191 84

  
192 85
/**
193 86
 * Gets the sensor reading for a robot to another robot.
194 87
 *
195
 * @param m the sensor matrix
196 88
 * @param observer the robot whose reading we check
197 89
 * @param robot the robot who we are checking the reading to
198 90
 *
199 91
 * @return the observer's BOM reading for robot
200 92
 **/
201
int sensor_matrix_get_reading(SensorMatrix* m, int observer, int robot)
93
int sensor_matrix_get_reading(int observer, int robot)
202 94
{
203
	if (observer >= m->size || robot >= m->size)
95
#ifndef BAYBOARD
96
	if (observer >= MAXIMUM_XBEE_ID || robot >= MAXIMUM_XBEE_ID)
204 97
		return -1;
205
	return m->matrix[observer][robot];
98

  
99
	return (int)m.matrix[observer][robot];
100
#else
101
	return -1;
102
#endif
206 103
}
207 104

  
208 105
/**
209 106
 * Sets whether or not the given robot is part of the token ring.
210 107
 *
211
 * @param m the sensor matrix
212 108
 * @param robot the robot to set as a member / nonmember of the token ring
213 109
 * @param in 1 if the robot is in the token ring, 0 otherwise
214 110
 **/
215
void sensor_matrix_set_in_ring(SensorMatrix* m, int robot, int in)
111
void sensor_matrix_set_in_ring(int robot, int in)
216 112
{
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;
113
	if (robot >= MAXIMUM_XBEE_ID)
114
	{
115
		WL_DEBUG_PRINT("ID too large.");
116
		return;
117
	}
118

  
119
	if (in == 1 && m.joined[robot] == 0)
120
		m.numJoined++;
121

  
122
	if (in == 0 && m.joined[robot])
123
		m.numJoined--;
124
	m.joined[robot] = in;
226 125
}
227 126

  
228 127
/**
229 128
 * Checks if the given robot is in the token ring.
230
 * 
231
 * @param m the sensor matrix
129
 *
232 130
 * @param robot the ID of the robot to check
233 131
 *
234 132
 * @return 1 if the robot is in the token ring, 0 otherwise
235 133
 **/
236
int sensor_matrix_get_in_ring(SensorMatrix* m, int robot)
134
int sensor_matrix_get_in_ring(int robot)
237 135
{
238
	if (robot >= m->size)
136
	if (robot >= MAXIMUM_XBEE_ID)
239 137
		return -1;
240
	return m->joined[robot];
138

  
139
	return m.joined[robot];
241 140
}
242 141

  
243 142
/**
244
 * Returns the size of the sensor matrix.
143
 * Returns the number of robots which have joined the
144
 * token ring.
245 145
 *
246
 * @param m the sensor matrix
247
 * 
248
 * @return the size of the sensor matrix
146
 * @return the number of robots in the token ring
249 147
 **/
250
int sensor_matrix_get_size(SensorMatrix* m)
148
int sensor_matrix_get_joined(void)
251 149
{
252
	return m->size;
150
	return m.numJoined;
253 151
}
254 152

  
255 153
/**
256
 * Returns the number of robots which have joined the
257
 * token ring.
154
 * Returns the maximum XBee id of a robot which can be stored
155
 * in the sensor matrix.
258 156
 *
259
 * @param m the sensor matrix
260
 *
261
 * @return the number of robots in the token ring
157
 * @return the maximum number of robots which can be stored
262 158
 **/
263
int sensor_matrix_get_joined(SensorMatrix* m)
159
int sensor_matrix_get_size(void)
264 160
{
265
	return m->numJoined;
161
	return MAXIMUM_XBEE_ID;
266 162
}
267 163

  

Also available in: Unified diff