Project

General

Profile

Statistics
| Revision:

root / trunk / code / lib / src / libdragonfly / bom.c @ 491

History | View | Annotate | Download (7.49 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
/**
28
 * @file bom.c
29
 * @brief Implementation for using the BOM
30
 *
31
 * Contains functions for using the Bearing and Orientation Module (BOM)
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 **/
35

    
36
#include <dragonfly_lib.h>
37
#include "bom.h"
38
#include "dio.h"
39
#include "analog.h"
40

    
41
//constants
42
static const char lookup[16] = {7,6,5,0xe,1,4,3,2,0xf,0,0xd,8,0xc,0xb,9,0xa};
43

    
44
// internal function prototypes
45
static void bom_select(char which);
46

    
47
/*
48
 Bk R Y (Analog)
49
---------
50
 Green
51
 Blue
52
 White
53
---------
54
 Blue
55
 White
56
*/
57

    
58

    
59
/*
60
the analog pin definitions from dio.h DO NOT work here,
61
so we must use PF0 from avrgcc (as opposed to _PIN_F0).
62
BUT the dio pin definitions from dio.h must be used (no PE...).
63

64
also, _PIN_E2 is initialized to high for some reason,
65
which turns the BOM on when the robot is turned on.
66
WORK-AROUND: call digital_output(_PIN_E2,0) at some point.
67

68
*/
69

    
70
#define MONKI PF0         //analog (yellow)
71
//------------------------//
72
#define MONKL _PIN_E2     //green
73
#define MONK1 _PIN_E3     //blue
74
#define MONK0 _PIN_E4     //white
75
//------------------------//
76
#define MONK3 _PIN_E6     //blue
77
#define MONK2 _PIN_E7     //white
78

    
79
#define BOM_VALUE_THRESHOLD 200
80
#define NUM_BOM_LEDS 16
81

    
82

    
83
/**
84
 * @defgroup bom BOM (Bearing and Orientation Module)
85
 * @brief Functions for dealing with the BOM.
86
 *
87
 * The Bearing and Orientation Module / Barrel of Monkeys / BOM
88
 * is a custom sensor designed and built by the Colony Project.
89
 * It consists of a ring of 16 IR emitters and 16 IR detectors.
90
 * The BOM is most often use to determine the direction of other
91
 * robots. This module contains functions for controlling the BOM.
92
 *
93
 * Include bom.h to access these functions.
94
 *
95
 * @{
96
 **/
97

    
98
static unsigned int bom_val[NUM_BOM_LEDS];
99
static char bom_type = BOM;
100
  
101
/**
102
 * Initializes the BOM.
103
 * Call bom_init before reading bom values or turning bom leds.
104
 *
105
 * @bugs INCOMPLETE - need to fill in init routine for BOM15
106
 * 
107
 * @see bom_refresh, bom_leds_on, bom_leds_off
108
 **/
109
void bom_init(char type) {
110
    bom_type = type;
111
    
112
    switch(bom_type) {
113
    case BOM:
114
        break;
115
    case BOM15:
116
        break;
117
    case RBOM:
118
        break;
119
    //default:
120
    }
121
}
122

    
123
/**
124
 * Iterates through each bit in the bit_field. For each set bit, sets the corresponding bom select bits
125
 *    and updates the corresponding bom value with an analog_get8 reading.  analog_init and bom_init
126
 *    must be called for this to work.
127
 *
128
 *
129
 * @param bit_field specifies which elements in bom_val[] should be updated. Use BOM_ALL to refresh all values.
130
 *    Ex. if 0x0003 is passed, bom_val[0] and bom_val[1] will be updated.
131
 *
132
 * @see bom_get
133
 **/
134
void bom_refresh(int bit_field) {
135
    int i;
136
    
137
    analog_stop_loop();
138
    
139
    for(i = 0; i < NUM_BOM_LEDS; i++) {
140
        if(bit_field & 0x1) {
141
            bom_select(lookup[i]);
142
            bom_val[i] = analog_get8(MONKI);
143
        }
144
        bit_field = bit_field >> 1;
145
    }
146
    
147
    analog_start_loop();
148
}
149

    
150
/**
151
 * Gets the bom reading from bom_val[which].  Call bom_refresh beforehand to read new bom values.
152
 *
153
 * @param which which bom value to return
154
 *
155
 * @return the bom value
156
 *
157
 * see bom_refresh
158
 **/
159
int bom_get(int which) {
160
    return bom_val[which];
161
}
162

    
163
/** 
164
 * Compares all the values in bom_val[] and returns the index to the lowest (max) value element.
165
 *
166
 * @return index to the lowest (max) bom value element.  -1 if no value is lower than
167
 *    BOM_VALUE_THRESHOLD
168
 **/
169
int bom_get_max(void) {
170
    int i, lowest_val, lowest_i;
171
    lowest_i = -1;
172
    lowest_val = 255;
173
    for(i = 0; i < NUM_BOM_LEDS; i++) {
174
        if(bom_val[i] < lowest_val) {
175
            lowest_val = bom_val[i];
176
            lowest_i = i;
177
        }
178
    }
179
    
180
    if(lowest_val < BOM_VALUE_THRESHOLD)
181
        return lowest_i;
182
    else
183
        return -1;
184
}
185

    
186
/**
187
 * Iterates through each bit in the bit_field. For each set bit, turns on the corresponding bom led.
188
 *    bom_init must be called for this to work. Only works with BOM_ALL if using the original bom.
189
 *
190
 * @param bit_field specifies which leds should be turned on.  Use BOM_ALL to turn on all bom leds.
191
 *    Ex. if 0x0005 is passed, leds 0 and 2 will be turned on.
192
 **/
193
void bom_leds_on(int bit_field) {
194
    switch(bom_type) {
195
    case BOM:
196
        if(bit_field == BOM_ALL) {
197
            digital_output(MONKL, 1);
198
        }
199
        break;
200
    case BOM15:
201
        //add bom 1.5 code here
202
        break;
203
    case RBOM:
204
        //add rbom code here
205
        break;
206
    }
207
}
208

    
209
/**
210
 * Iterates through each bit in the bit_field. For each set bit, turns off the corresponding bom led.
211
 *    bom_init must be called for this to work. Only works with BOM_ALL if using the original bom.
212
 *
213
 * @param bit_field specifies which leds should be turned off.  Use BOM_ALL to turn off all bom leds.
214
 *    Ex. if 0x000B is passed, leds 0 and 3 will be turned off.
215
 **/
216
void bom_leds_off(int bit_field) {
217
    switch(bom_type) {
218
    case BOM:
219
        if(bit_field == BOM_ALL) {
220
            digital_output(MONKL, 0);
221
        }
222
        break;
223
    case BOM15:
224
        //add bom 1.5 code here
225
        break;
226
    case RBOM:
227
        //add rbom code here
228
        break;
229
    }
230
}
231

    
232

    
233
/**
234
 * (DEPRECATED) Returns the direction of the maximum BOM reading,
235
 * as an integer in the range 0-15. 0 indicates to the
236
 * robot's right, while the rest of the sensors are
237
 * numbered counterclockwise. This is useful for determining
238
 * the direction of a robot flashing its BOM, of only one
239
 * robot is currently doing so. analog_init must be called
240
 * before this function can be used.
241
 *
242
 * @return the direction of the maximum BOM reading
243
 *
244
 * @see analog_init
245
 **/
246
int get_max_bom(void) {
247
    bom_refresh(BOM_ALL);
248
    return bom_get_max();
249
}
250

    
251
/**
252
 * (DEPRECATED) Turns on all bom leds.
253
 * 
254
 * @see bom_off
255
 **/
256
void bom_on(void)
257
{
258
  bom_leds_on(BOM_ALL);
259
}
260

    
261
/**
262
 * (DEPRECATED) Turns off all bom leds.
263
 * 
264
 * @see bom_on
265
 **/
266
void bom_off(void)
267
{
268
    bom_leds_off(BOM_ALL);
269
}
270

    
271
/** @} **/ //end group
272

    
273
static void bom_select(char which) {
274
      if (which&8)
275
        digital_output(MONK3, 1);
276
      else
277
        digital_output(MONK3, 0);
278

    
279
      if (which&4)
280
        digital_output(MONK2, 1);
281
      else
282
        digital_output(MONK2, 0);
283

    
284
      if (which&2)
285
        digital_output(MONK1, 1);
286
      else
287
        digital_output(MONK1, 0);
288

    
289
      if (which&1)
290
        digital_output(MONK0, 1);
291
      else
292
        digital_output(MONK0, 0);
293
}