Statistics
| Revision:

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

 1 ```#include ``` ```#include ``` ```#include ``` ```#include "sensor_matrix.h" ``` ```#define DEFAULT_SENSOR_MATRIX_SIZE 20 ``` ```/*Sensor Matrix Functions*/ ``` ```void sensor_matrix_expand(SensorMatrix* m, int nextSize); ``` ```/** ``` ``` * Initializes the sensor matrix. ``` ``` * ``` ``` * @return the newly created sensor matrix ``` ``` **/ ``` ```SensorMatrix* sensor_matrix_create() ``` ```{ ``` ``` SensorMatrix* m; ``` ``` int i; ``` ``` ``` ``` m = (SensorMatrix*)malloc(sizeof(SensorMatrix)); ``` ``` if (!m) ``` ``` { ``` ``` WL_DEBUG_PRINT("Out of memory - create sensor matrix.\r\n"); ``` ``` return NULL; ``` ``` } ``` ``` m->size = DEFAULT_SENSOR_MATRIX_SIZE; ``` ``` m->matrix = (int**)malloc(m->size * sizeof(int*)); ``` ``` m->joined = (int*)malloc(m->size * sizeof(int)); ``` ``` m->numJoined = 0; ``` ``` if (!(m->matrix) || !(m->joined)) ``` ``` { ``` ``` WL_DEBUG_PRINT("Out of memory - create sensor matrix 2.\r\n"); ``` ``` return NULL; ``` ``` } ``` ``` ``` ``` for (i = 0; i < m->size; i++) ``` ``` { ``` ``` m->matrix[i] = NULL; ``` ``` m->joined[i] = 0; ``` ``` } ``` ``` return m; ``` ```} ``` ```/** ``` ``` * Deletes and frees memory from the sensor matrix. ``` ``` * ``` ``` * @param m the sensor matrix to delete ``` ``` **/ ``` ```void sensor_matrix_destroy(SensorMatrix* m) ``` ```{ ``` ``` int i; ``` ``` for (i = 0; i < m->size; i++) ``` ``` if (m->matrix[i] != NULL) ``` ``` free(m->matrix[i]); ``` ``` free(m->matrix); ``` ``` free(m->joined); ``` ``` free(m); ``` ```} ``` ```/** ``` ``` * Adds robot with XBee id id to the sensor matrix. ``` ``` * ``` ``` * @param m the sensor matrix ``` ``` * @param id the XBee ID of the robot to add ``` ``` **/ ``` ```void sensor_matrix_add_robot(SensorMatrix* m, unsigned int id) ``` ```{ ``` ``` int i; ``` ``` if (id >= m->size) ``` ``` sensor_matrix_expand(m, id + 1); ``` ``` if (m->matrix[id] != NULL) ``` ``` return; ``` ``` ``` ``` m->matrix[id] = (int*)malloc(m->size * sizeof(int)); ``` ``` if (!(m->matrix[id])) ``` ``` { ``` ``` WL_DEBUG_PRINT("Out of memory - add robot.\r\n"); ``` ``` return; ``` ``` } ``` ``` for (i = 0; i < m->size; i++) ``` ``` if (m->matrix[i] != NULL) ``` ``` m->matrix[i][id] = -1; ``` ```} ``` ```/** ``` ``` * Removes robot with id from the sensor matrix, and removes ``` ``` * all sensor information regarding the robot. ``` ``` * ``` ``` * @param m the sensor matrix ``` ``` * @param id the XBee ID of the robot to remove ``` ``` **/ ``` ```void sensor_matrix_remove_robot(SensorMatrix* m, unsigned int id) ``` ```{ ``` ``` int i; ``` ``` if (id >= m->size || m->matrix[id] == NULL) ``` ``` { ``` ``` WL_DEBUG_PRINT("Removing robot not added to matrix.\r\n"); ``` ``` return; ``` ``` } ``` ``` ``` ``` free(m->matrix[id]); ``` ``` m->matrix[id] = NULL; ``` ``` ``` ``` for (i = 0 ; i < m->size; i++) ``` ``` if (m->matrix[i] != NULL) ``` ``` m->matrix[i][id] = -1; ``` ``` ``` ``` m->joined[id] = 0; ``` ```} ``` ```/** ``` ``` * Expands the size of the sensor matrix if an id number we attempt ``` ``` * to add is too large. ``` ``` * ``` ``` * @param m the sensor matrix to expand ``` ``` * @param size the new size of the sensor matrix ``` ``` **/ ``` ```//Note: this has probably not been tested, hopefully it works ``` ```void sensor_matrix_expand(SensorMatrix* m, int nextSize) ``` ```{ ``` ``` int i, j; ``` ``` WL_DEBUG_PRINT("Expanding sensor matrix.\r\n"); ``` ``` ``` ``` int** tempMatrix = (int**)malloc(nextSize * sizeof(int*)); ``` ``` if (!tempMatrix) ``` ``` { ``` ``` WL_DEBUG_PRINT("Out of memory - expand matrix.\r\n"); ``` ``` return; ``` ``` } ``` ``` ``` ``` for (i = 0; i < nextSize; i++) ``` ``` tempMatrix[i] = NULL; ``` ``` ``` ``` //copy over old sensor data ``` ``` for (i = 0; i < m->size; i++) ``` ``` if (m->matrix[i] != NULL) ``` ``` { ``` ``` tempMatrix[i] = (int *)malloc(nextSize * sizeof(int)); ``` ``` if (!tempMatrix[i]) ``` ``` { ``` ``` WL_DEBUG_PRINT("Out of memory - expand matrix 2.\r\n"); ``` ``` return; ``` ``` } ``` ``` for (j = 0; j < m->size; j++) ``` ``` tempMatrix[i][j] = m->matrix[i][j]; ``` ``` for (j = m->size; j < nextSize; j++) ``` ``` tempMatrix[i][j] = -1; ``` ``` ``` ``` free(m->matrix[i]); ``` ``` } ``` ``` ``` ``` free(m->matrix); ``` ``` m->matrix = tempMatrix; ``` ``` m->size = nextSize; ``` ``` //expand the size of joined ``` ``` int* tempJoined = (int *)malloc(nextSize * sizeof(int)); ``` ``` if (!tempJoined) ``` ``` { ``` ``` WL_DEBUG_PRINT("Out of memory - expand matrix 3.\r\n"); ``` ``` return; ``` ``` } ``` ``` ``` ``` for (i = 0; i < m->size; i++) ``` ``` tempJoined[i] = m->joined[i]; ``` ``` for (i = m->size; i < nextSize; i++) ``` ``` tempJoined[i] = 0; ``` ``` ``` ``` free(m->joined); ``` ``` m->joined = tempJoined; ``` ```} ``` ```/** ``` ``` * Sets the sensor reading for robot robot to reading. ``` ``` * ``` ``` * @param m the sensor matrix to set the reading for ``` ``` * @param observer the id of the robot who made the reading ``` ``` * @param robot the id of the robot who the reading is for ``` ``` * @param reading the BOM reading from observer to robot ``` ``` */ ``` ```void sensor_matrix_set_reading(SensorMatrix* m, int observer, int robot, int reading) ``` ```{ ``` ``` if (robot >= m->size || observer >= m->size || m->matrix[observer] == NULL) ``` ``` sensor_matrix_add_robot(m, observer); ``` ``` m->matrix[observer][robot] = reading; ``` ```} ``` ```/** ``` ``` * Gets the sensor reading for a robot to another robot. ``` ``` * ``` ``` * @param m the sensor matrix ``` ``` * @param observer the robot whose reading we check ``` ``` * @param robot the robot who we are checking the reading to ``` ``` * ``` ``` * @return the observer's BOM reading for robot ``` ``` **/ ``` ```int sensor_matrix_get_reading(SensorMatrix* m, int observer, int robot) ``` ```{ ``` ``` if (observer >= m->size || robot >= m->size) ``` ``` return -1; ``` ``` return m->matrix[observer][robot]; ``` ```} ``` ```/** ``` ``` * Sets whether or not the given robot is part of the token ring. ``` ``` * ``` ``` * @param m the sensor matrix ``` ``` * @param robot the robot to set as a member / nonmember of the token ring ``` ``` * @param in 1 if the robot is in the token ring, 0 otherwise ``` ``` **/ ``` ```void sensor_matrix_set_in_ring(SensorMatrix* m, int robot, int in) ``` ```{ ``` ``` if (robot >= m->size) ``` ``` sensor_matrix_expand(m, robot + 1); ``` ``` if (in == 1) ``` ``` sensor_matrix_add_robot(m, robot); ``` ``` if (in == 1 && m->joined[robot] == 0) ``` ``` m->numJoined++; ``` ``` if (in == 0 && m->joined[robot] == 1) ``` ``` m->numJoined--; ``` ``` m->joined[robot] = in; ``` ```} ``` ```/** ``` ``` * Checks if the given robot is in the token ring. ``` ``` * ``` ``` * @param m the sensor matrix ``` ``` * @param robot the ID of the robot to check ``` ``` * ``` ``` * @return 1 if the robot is in the token ring, 0 otherwise ``` ``` **/ ``` ```int sensor_matrix_get_in_ring(SensorMatrix* m, int robot) ``` ```{ ``` ``` if (robot >= m->size) ``` ``` return -1; ``` ``` return m->joined[robot]; ``` ```} ``` ```/** ``` ``` * Returns the size of the sensor matrix. ``` ``` * ``` ``` * @param m the sensor matrix ``` ``` * ``` ``` * @return the size of the sensor matrix ``` ``` **/ ``` ```int sensor_matrix_get_size(SensorMatrix* m) ``` ```{ ``` ``` return m->size; ``` ```} ``` ```/** ``` ``` * Returns the number of robots which have joined the ``` ``` * token ring. ``` ``` * ``` ``` * @param m the sensor matrix ``` ``` * ``` ``` * @return the number of robots in the token ring ``` ``` **/ ``` ```int sensor_matrix_get_joined(SensorMatrix* m) ``` ```{ ``` ``` return m->numJoined; ``` ```} ```