root / trunk / code / projects / libdragonfly / bom.c @ 448
History | View | Annotate | Download (7.9 KB)
1 | 241 | bcoltin | /**
|
---|---|---|---|
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 | 8 | bcoltin | #include <dragonfly_lib.h> |
37 | #include "bom.h" |
||
38 | #include "dio.h" |
||
39 | #include "analog.h" |
||
40 | |||
41 | 448 | cmar | //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 | 8 | bcoltin | //constants
|
61 | 448 | cmar | static const char lookup[16] = {7,6,5,0xe,1,4,3,2,0xf,0,0xd,8,0xc,0xb,9,0xa}; |
62 | 8 | bcoltin | |
63 | // internal function prototypes
|
||
64 | 448 | cmar | static void bom_select(char which); |
65 | 8 | bcoltin | |
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 | 448 | cmar | #define BOM_VALUE_THRESHOLD 200 |
99 | #define NUM_BOM_LEDS 16 |
||
100 | 8 | bcoltin | |
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 | 448 | cmar | **/
|
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 | 8 | bcoltin | |
254 | 448 | cmar | |
255 | 8 | bcoltin | /**
|
256 | 448 | cmar | * (DEPRECATED) Returns the direction of the maximum BOM reading,
|
257 | 8 | bcoltin | * 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 | 448 | cmar | bom_refresh(BOM_ALL); |
270 | return bom_get_max();
|
||
271 | 8 | bcoltin | } |
272 | |||
273 | /**
|
||
274 | 448 | cmar | * (DEPRECATED) Turns on all bom leds.
|
275 | 8 | bcoltin | *
|
276 | 448 | cmar | * @see bom_off
|
277 | 8 | bcoltin | **/
|
278 | void bom_on(void) |
||
279 | { |
||
280 | 448 | cmar | bom_leds_on(BOM_ALL); |
281 | 8 | bcoltin | } |
282 | |||
283 | /**
|
||
284 | 448 | cmar | * (DEPRECATED) Turns off all bom leds.
|
285 | 8 | bcoltin | *
|
286 | 448 | cmar | * @see bom_on
|
287 | 8 | bcoltin | **/
|
288 | void bom_off(void) |
||
289 | { |
||
290 | 448 | cmar | bom_leds_off(BOM_ALL); |
291 | 8 | bcoltin | } |
292 | |||
293 | /** @} **/ //end group |
||
294 | 448 | cmar | |
295 | static void bom_select(char which) { |
||
296 | if (which&8) |
||
297 | digital_output(MONK3, 1);
|
||
298 | else
|
||
299 | digital_output(MONK3, 0);
|
||
300 | 8 | bcoltin | |
301 | 448 | cmar | if (which&4) |
302 | digital_output(MONK2, 1);
|
||
303 | else
|
||
304 | digital_output(MONK2, 0);
|
||
305 | 8 | bcoltin | |
306 | 448 | cmar | if (which&2) |
307 | digital_output(MONK1, 1);
|
||
308 | else
|
||
309 | digital_output(MONK1, 0);
|
||
310 | 8 | bcoltin | |
311 | 448 | cmar | if (which&1) |
312 | digital_output(MONK0, 1);
|
||
313 | else
|
||
314 | digital_output(MONK0, 0);
|
||
315 | } |