Colony

/**

 * Copyright (c) 2007 Colony Project

 *

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 *

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 *

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file sensor_matrix.c

 * @brief Sensor Matrix implementation

 *

 * Implementation of a sensor matrix for storing localization implementation.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

#include <stdlib.h>

#include <stdio.h>

#include <wl_defs.h>

#include "sensor_matrix.h"

// the global sensor matrix

SensorMatrix m;

/**

 * Initializes the sensor matrix.

 *

 * @return the newly created sensor matrix

 **/

void sensor_matrix_create()

{

        int i, j;

        m.numJoined = 0;

        for (i = 0; i < MAXIMUM_XBEE_ID; i++)

        {

                m.joined[i] = 0;

                for (j = 0; j < MAXIMUM_XBEE_ID; j++)

                        m.matrix[i][j] = READING_UNKNOWN;

        }

}

/**

 * Sets the sensor reading for robot robot to reading.

 *

 * @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(int observer, int robot, int reading)

{

        if (robot >= MAXIMUM_XBEE_ID || observer >= MAXIMUM_XBEE_ID)

        {

                WL_DEBUG_PRINT("ID too large.");

                return;

        }

        m.matrix[observer][robot] = (unsigned char)reading;

}

/**

 * Gets the sensor reading for a robot to another robot.

 *

 * @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(int observer, int robot)

{

        if (observer >= MAXIMUM_XBEE_ID || robot >= MAXIMUM_XBEE_ID)

                return -1;

        return (int)m.matrix[observer][robot];

}

/**

 * Sets whether or not the given robot is part of the token ring.

 *

 * @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(int robot, int in)

{

        if (robot >= MAXIMUM_XBEE_ID)

        {

                WL_DEBUG_PRINT("ID too large.");

                return;

        }

        if (in == 1 && m.joined[robot] == 0)

                m.numJoined++;

        if (in == 0 && m.joined[robot])

                m.numJoined--;

        m.joined[robot] = in;

}

/**

 * Checks if the given robot is in the token ring.

 *

 * @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(int robot)

{

        if (robot >= MAXIMUM_XBEE_ID)

                return -1;

        return m.joined[robot];

}

/**

 * Returns the number of robots which have joined the

 * token ring.

 *

 * @return the number of robots in the token ring

 **/

int sensor_matrix_get_joined(void)

{

        return m.numJoined;

}

/**

 * Returns the maximum XBee id of a robot which can be stored

 * in the sensor matrix.

 *

 * @return the maximum number of robots which can be stored

 **/

int sensor_matrix_get_size(void)

{

        return MAXIMUM_XBEE_ID;

}