Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / libwireless / lib / sensor_matrix.c @ 887

History | View | Annotate | Download (3.85 KB)

1
/**
2
 * Copyright (c) 2007 Colony Project
3
 *
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.
24
 **/
25

    
26
/**
27
 * @file sensor_matrix.c
28
 * @brief Sensor Matrix implementation
29
 *
30
 * Implementation of a sensor matrix for storing localization implementation.
31
 *
32
 * @author Brian Coltin, Colony Project, CMU Robotics Club
33
 **/
34

    
35
#include <stdlib.h>
36
#include <stdio.h>
37
#include <wl_defs.h>
38

    
39
#include "sensor_matrix.h"
40

    
41
// the global sensor matrix
42
SensorMatrix m;
43

    
44
/**
45
 * Initializes the sensor matrix.
46
 *
47
 * @return the newly created sensor matrix
48
 **/
49
void sensor_matrix_create()
50
{
51
        int i, j;
52

    
53
        m.numJoined = 0;
54

    
55
        for (i = 0; i < MAXIMUM_XBEE_ID; i++)
56
        {
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
62
        }
63
}
64

    
65
/**
66
 * Sets the sensor reading for robot robot to reading.
67
 *
68
 * @param observer the id of the robot who made the reading
69
 * @param robot the id of the robot who the reading is for
70
 * @param reading the BOM reading from observer to robot
71
 */
72
void sensor_matrix_set_reading(int observer, int robot, int reading)
73
{
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
83
}
84

    
85
/**
86
 * Gets the sensor reading for a robot to another robot.
87
 *
88
 * @param observer the robot whose reading we check
89
 * @param robot the robot who we are checking the reading to
90
 *
91
 * @return the observer's BOM reading for robot
92
 **/
93
int sensor_matrix_get_reading(int observer, int robot)
94
{
95
#ifndef BAYBOARD
96
        if (observer >= MAXIMUM_XBEE_ID || robot >= MAXIMUM_XBEE_ID)
97
                return -1;
98

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

    
105
/**
106
 * Sets whether or not the given robot is part of the token ring.
107
 *
108
 * @param robot the robot to set as a member / nonmember of the token ring
109
 * @param in 1 if the robot is in the token ring, 0 otherwise
110
 **/
111
void sensor_matrix_set_in_ring(int robot, int in)
112
{
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;
125
}
126

    
127
/**
128
 * Checks if the given robot is in the token ring.
129
 *
130
 * @param robot the ID of the robot to check
131
 *
132
 * @return 1 if the robot is in the token ring, 0 otherwise
133
 **/
134
int sensor_matrix_get_in_ring(int robot)
135
{
136
        if (robot >= MAXIMUM_XBEE_ID)
137
                return -1;
138

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

    
142
/**
143
 * Returns the number of robots which have joined the
144
 * token ring.
145
 *
146
 * @return the number of robots in the token ring
147
 **/
148
int sensor_matrix_get_joined(void)
149
{
150
        return m.numJoined;
151
}
152

    
153
/**
154
 * Returns the maximum XBee id of a robot which can be stored
155
 * in the sensor matrix.
156
 *
157
 * @return the maximum number of robots which can be stored
158
 **/
159
int sensor_matrix_get_size(void)
160
{
161
        return MAXIMUM_XBEE_ID;
162
}
163