Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (7.9 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
//TODO: DELETE THIS
42
void printi(int i)
43
{
44
    char c3 = (i % 10) + '0';
45
    i /= 10;
46
    char c2 = (i % 10) + '0';
47
    i /= 10;
48
    char c1 = (i % 10) + '0';
49
    if (c1 == '0')
50
    {
51
        c1 = ' ';
52
        if (c2 == '0')
53
            c2 = ' ';
54
    }
55
    usb_putc(c1);
56
    usb_putc(c2);
57
    usb_putc(c3);
58
}
59

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

    
63
// internal function prototypes
64
static void bom_select(char which);
65

    
66
/*
67
 Bk R Y (Analog)
68
---------
69
 Green
70
 Blue
71
 White
72
---------
73
 Blue
74
 White
75
*/
76

    
77

    
78
/*
79
the analog pin definitions from dio.h DO NOT work here,
80
so we must use PF0 from avrgcc (as opposed to _PIN_F0).
81
BUT the dio pin definitions from dio.h must be used (no PE...).
82

83
also, _PIN_E2 is initialized to high for some reason,
84
which turns the BOM on when the robot is turned on.
85
WORK-AROUND: call digital_output(_PIN_E2,0) at some point.
86

87
*/
88

    
89
#define MONKI PF0         //analog (yellow)
90
//------------------------//
91
#define MONKL _PIN_E2     //green
92
#define MONK1 _PIN_E3     //blue
93
#define MONK0 _PIN_E4     //white
94
//------------------------//
95
#define MONK3 _PIN_E6     //blue
96
#define MONK2 _PIN_E7     //white
97

    
98
#define BOM_VALUE_THRESHOLD 200
99
#define NUM_BOM_LEDS 16
100

    
101

    
102
/**
103
 * @defgroup bom BOM (Bearing and Orientation Module)
104
 * @brief Functions for dealing with the BOM.
105
 *
106
 * The Bearing and Orientation Module / Barrel of Monkeys / BOM
107
 * is a custom sensor designed and built by the Colony Project.
108
 * It consists of a ring of 16 IR emitters and 16 IR detectors.
109
 * The BOM is most often use to determine the direction of other
110
 * robots. This module contains functions for controlling the BOM.
111
 *
112
 * Include bom.h to access these functions.
113
 *
114
 * @{
115
 **/
116

    
117
static unsigned int bom_val[NUM_BOM_LEDS];
118
static char bom_type = BOM;
119
  
120
/**
121
 * Initializes the BOM.
122
 * Call bom_init before reading bom values or turning bom leds.
123
 *
124
 * @bugs INCOMPLETE - need to fill in init routine for BOM15
125
 * 
126
 * @see bom_refresh, bom_leds_on, bom_leds_off
127
 **/
128
void bom_init(char type) {
129
    bom_type = type;
130
    
131
    switch(bom_type) {
132
    case BOM:
133
        break;
134
    case BOM15:
135
        break;
136
    case RBOM:
137
        break;
138
    //default:
139
    }
140
}
141

    
142
/**
143
 * Iterates through each bit in the bit_field. For each set bit, sets the corresponding bom select bits
144
 *    and updates the corresponding bom value with an analog_get8 reading.  analog_init and bom_init
145
 *    must be called for this to work.
146
 *
147
 *
148
 * @param bit_field specifies which elements in bom_val[] should be updated. Use BOM_ALL to refresh all values.
149
 *    Ex. if 0x0003 is passed, bom_val[0] and bom_val[1] will be updated.
150
 *
151
 * @see bom_get
152
 **/
153
void bom_refresh(int bit_field) {
154
    int i;
155
    
156
    analog_stop_loop();
157
    
158
    for(i = 0; i < NUM_BOM_LEDS; i++) {
159
        if(bit_field & 0x1) {
160
            bom_select(lookup[i]);
161
            bom_val[i] = analog_get8(MONKI);
162
        }
163
        bit_field = bit_field >> 1;
164
    }
165
    
166
    analog_start_loop();
167
}
168

    
169
/**
170
 * Gets the bom reading from bom_val[which].  Call bom_refresh beforehand to read new bom values.
171
 *
172
 * @param which which bom value to return
173
 *
174
 * @return the bom value
175
 *
176
 * see bom_refresh
177
 **/
178
int bom_get(int which) {
179
    return bom_val[which];
180
}
181

    
182
/** 
183
 * Compares all the values in bom_val[] and returns the index to the lowest (max) value element.
184
 *
185
 * @return index to the lowest (max) bom value element.  -1 if no value is lower than
186
 *    BOM_VALUE_THRESHOLD
187
 **/
188
int bom_get_max(void) {
189
    int i, lowest_val, lowest_i;
190
    lowest_i = -1;
191
    lowest_val = 255;
192
    for(i = 0; i < NUM_BOM_LEDS; i++) {
193
        printi(bom_val[i]);
194
        usb_puts(" ");
195
        if(bom_val[i] < lowest_val) {
196
            lowest_val = bom_val[i];
197
            lowest_i = i;
198
        }
199
    }
200
    usb_puts("<< BOM_GET_MAX\n\r");
201
    
202
    if(lowest_val < BOM_VALUE_THRESHOLD)
203
        return lowest_i;
204
    else
205
        return -1;
206
}
207

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

    
231
/**
232
 * Iterates through each bit in the bit_field. For each set bit, turns off the corresponding bom led.
233
 *    bom_init must be called for this to work. Only works with BOM_ALL if using the original bom.
234
 *
235
 * @param bit_field specifies which leds should be turned off.  Use BOM_ALL to turn off all bom leds.
236
 *    Ex. if 0x000B is passed, leds 0 and 3 will be turned off.
237
 **/
238
void bom_leds_off(int bit_field) {
239
    switch(bom_type) {
240
    case BOM:
241
        if(bit_field == BOM_ALL) {
242
            digital_output(MONKL, 0);
243
        }
244
        break;
245
    case BOM15:
246
        //add bom 1.5 code here
247
        break;
248
    case RBOM:
249
        //add rbom code here
250
        break;
251
    }
252
}
253

    
254

    
255
/**
256
 * (DEPRECATED) Returns the direction of the maximum BOM reading,
257
 * as an integer in the range 0-15. 0 indicates to the
258
 * robot's right, while the rest of the sensors are
259
 * numbered counterclockwise. This is useful for determining
260
 * the direction of a robot flashing its BOM, of only one
261
 * robot is currently doing so. analog_init must be called
262
 * before this function can be used.
263
 *
264
 * @return the direction of the maximum BOM reading
265
 *
266
 * @see analog_init
267
 **/
268
int get_max_bom(void) {
269
    bom_refresh(BOM_ALL);
270
    return bom_get_max();
271
}
272

    
273
/**
274
 * (DEPRECATED) Turns on all bom leds.
275
 * 
276
 * @see bom_off
277
 **/
278
void bom_on(void)
279
{
280
  bom_leds_on(BOM_ALL);
281
}
282

    
283
/**
284
 * (DEPRECATED) Turns off all bom leds.
285
 * 
286
 * @see bom_on
287
 **/
288
void bom_off(void)
289
{
290
    bom_leds_off(BOM_ALL);
291
}
292

    
293
/** @} **/ //end group
294

    
295
static void bom_select(char which) {
296
      if (which&8)
297
        digital_output(MONK3, 1);
298
      else
299
        digital_output(MONK3, 0);
300

    
301
      if (which&4)
302
        digital_output(MONK2, 1);
303
      else
304
        digital_output(MONK2, 0);
305

    
306
      if (which&2)
307
        digital_output(MONK1, 1);
308
      else
309
        digital_output(MONK1, 0);
310

    
311
      if (which&1)
312
        digital_output(MONK0, 1);
313
      else
314
        digital_output(MONK0, 0);
315
}