Statistics
| Revision:

root / demos / smart_run_around_fsm / lib / src / libwireless / sensor_matrix.c @ 1784

History | View | Annotate | Download (3.81 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 "wl_defs.h"
36
#include "sensor_matrix.h"
37

    
38
// the global sensor matrix
39
SensorMatrix m;
40

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

    
50
        m.numJoined = 0;
51

    
52
        for (i = 0; i < MAXIMUM_XBEE_ID; i++)
53
        {
54
                m.joined[i] = 0;
55
#ifndef BAYBOARD
56
                for (j = 0; j < MAXIMUM_XBEE_ID; j++)
57
                        m.matrix[i][j] = READING_UNKNOWN;
58
#endif
59
        }
60
}
61

    
62
/**
63
 * Sets the sensor reading for robot robot to reading.
64
 *
65
 * @param observer the id of the robot who made the reading
66
 * @param robot the id of the robot who the reading is for
67
 * @param reading the BOM reading from observer to robot
68
 */
69
void sensor_matrix_set_reading(int observer, int robot, int reading)
70
{
71
#ifndef BAYBOARD
72
        if (robot >= MAXIMUM_XBEE_ID || observer >= MAXIMUM_XBEE_ID)
73
        {
74
                WL_DEBUG_PRINT("ID too large.");
75
                return;
76
        }
77

    
78
        m.matrix[observer][robot] = (unsigned char)reading;
79
#endif
80
}
81

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

    
96
        return (int)m.matrix[observer][robot];
97
#else
98
        return -1;
99
#endif
100
}
101

    
102
/**
103
 * Sets whether or not the given robot is part of the token ring.
104
 *
105
 * @param robot the robot to set as a member / nonmember of the token ring
106
 * @param in 1 if the robot is in the token ring, 0 otherwise
107
 **/
108
void sensor_matrix_set_in_ring(int robot, int in)
109
{
110
        if (robot >= MAXIMUM_XBEE_ID)
111
        {
112
                WL_DEBUG_PRINT("ID too large.");
113
                return;
114
        }
115

    
116
        if (in == 1 && m.joined[robot] == 0)
117
                m.numJoined++;
118

    
119
        if (in == 0 && m.joined[robot])
120
                m.numJoined--;
121
        m.joined[robot] = in;
122
}
123

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

    
136
        return m.joined[robot];
137
}
138

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

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