root / trunk / code / projects / libdragonfly / bom.c @ 1461
History | View | Annotate | Download (10.8 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 | 1461 | bneuman | * 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 "bom.h" |
||
37 | #include "dio.h" |
||
38 | #include "serial.h" |
||
39 | #include "analog.h" |
||
40 | |||
41 | //On the original BOM1.0, the emmitter angular order does not match the analog mux order
|
||
42 | //so you need to iterate through the mux index in the following order if you want to get
|
||
43 | //the detector readings in order:
|
||
44 | static const char lookup[16] = {7,6,5,0xe,1,4,3,2,0xf,0,0xd,8,0xc,0xb,9,0xa}; |
||
45 | |||
46 | // internal function prototypes
|
||
47 | static void bom_select(char which); |
||
48 | |||
49 | /*
|
||
50 | Bk R Y (Analog)
|
||
51 | ---------
|
||
52 | Green
|
||
53 | Blue
|
||
54 | White
|
||
55 | ---------
|
||
56 | Blue
|
||
57 | White
|
||
58 | */
|
||
59 | |||
60 | |||
61 | /*
|
||
62 | the analog pin definitions from dio.h DO NOT work here,
|
||
63 | so we must use PF0 from avrgcc (as opposed to _PIN_F0).
|
||
64 | BUT the dio pin definitions from dio.h must be used (no PE...).
|
||
65 | |||
66 | also, _PIN_E2 is initialized to high for some reason,
|
||
67 | which turns the BOM on when the robot is turned on.
|
||
68 | WORK-AROUND: call digital_output(_PIN_E2,0) at some point.
|
||
69 | |||
70 | */
|
||
71 | |||
72 | #define MONKI PF0 //analog (yellow) |
||
73 | //------------------------//
|
||
74 | #define MONKL _PIN_E2 //green |
||
75 | #define MONK1 _PIN_E3 //blue |
||
76 | #define MONK0 _PIN_E4 //white |
||
77 | //------------------------//
|
||
78 | #define MONK3 _PIN_E6 //blue |
||
79 | #define MONK2 _PIN_E7 //white |
||
80 | |||
81 | #define BOM_VALUE_THRESHOLD 150 //200 |
||
82 | #define NUM_BOM_LEDS 16 |
||
83 | |||
84 | /*
|
||
85 | *The following pin definitions are for the BOM v1.5
|
||
86 | */
|
||
87 | |||
88 | #define BOM_MODE _PIN_E2 //dio0 |
||
89 | #define BOM_STROBE _PIN_E3 //dio1 |
||
90 | |||
91 | #define BOM_DATA _PIN_A0 //servo0 |
||
92 | #define BOM_CLOCK _PIN_A1 //servo1 |
||
93 | |||
94 | #define BOM_S0 _PIN_E5 //dio3 |
||
95 | #define BOM_S1 _PIN_E4 //dio2 |
||
96 | #define BOM_S2 _PIN_E7 //dio4 |
||
97 | #define BOM_S3 _PIN_E6 //dio5 |
||
98 | #define BOM_OUT PF0 //analog(yellow) |
||
99 | |||
100 | /**
|
||
101 | * @defgroup bom BOM (Bearing and Orientation Module)
|
||
102 | * @brief Functions for dealing with the BOM.
|
||
103 | *
|
||
104 | * The Bearing and Orientation Module / Barrel of Monkeys / BOM
|
||
105 | * is a custom sensor designed and built by the Colony Project.
|
||
106 | * It consists of a ring of 16 IR emitters and 16 IR detectors.
|
||
107 | * The BOM is most often use to determine the direction of other
|
||
108 | * robots. This module contains functions for controlling the BOM.
|
||
109 | *
|
||
110 | * Include bom.h to access these functions.
|
||
111 | *
|
||
112 | * @{
|
||
113 | **/
|
||
114 | |||
115 | unsigned char bom_initd=0; |
||
116 | |||
117 | static unsigned int bom_val[NUM_BOM_LEDS]; |
||
118 | static volatile char bom_type = BOM10; |
||
119 | static int select_pins[4]; |
||
120 | static int analog_pin; |
||
121 | |||
122 | /**
|
||
123 | * Initializes the BOM.
|
||
124 | * Call bom_init before reading bom values or turning bom leds.
|
||
125 | *
|
||
126 | * @bugs INCOMPLETE - No utilization of BOM1.5 RSSI capability. Probably leave this out
|
||
127 | * until Cornell and Pras return
|
||
128 | *
|
||
129 | * @return 0 if init succesfull, an error code otherwise
|
||
130 | *
|
||
131 | * @see bom_refresh, bom_leds_on, bom_leds_off
|
||
132 | **/
|
||
133 | int bom_init(char type) { |
||
134 | |||
135 | if(bom_initd)
|
||
136 | return ERROR_INIT_ALREADY_INITD;
|
||
137 | |||
138 | |||
139 | bom_type = type; |
||
140 | |||
141 | switch(bom_type) {
|
||
142 | case BOM10:
|
||
143 | select_pins[0] = MONK0;
|
||
144 | select_pins[1] = MONK1;
|
||
145 | select_pins[2] = MONK2;
|
||
146 | select_pins[3] = MONK3;
|
||
147 | analog_pin = MONKI; |
||
148 | break;
|
||
149 | case BOM15:
|
||
150 | //Sets BOM1.5 to normal [BOM] mode
|
||
151 | digital_output(BOM_MODE, 0);
|
||
152 | select_pins[0] = BOM_S0;
|
||
153 | select_pins[1] = BOM_S1;
|
||
154 | select_pins[2] = BOM_S2;
|
||
155 | select_pins[3] = BOM_S3;
|
||
156 | bom_set_leds(BOM_ALL); |
||
157 | analog_pin = BOM_OUT; |
||
158 | break;
|
||
159 | case RBOM:
|
||
160 | break;
|
||
161 | //default:
|
||
162 | } |
||
163 | |||
164 | bom_initd=1;
|
||
165 | return 0; |
||
166 | 1443 | rcahoon | } |
167 | |||
168 | 1461 | bneuman | /**
|
169 | * Iterates through each bit in the bit_field. For each set bit, sets the corresponding bom select bits
|
||
170 | * and updates the corresponding bom value with an analog_get8 reading. analog_init and bom_init
|
||
171 | * must be called for this to work. Must call this before reading BOM values!
|
||
172 | *
|
||
173 | *
|
||
174 | * @param bit_field specifies which elements in bom_val[] should be updated. Use BOM_ALL to refresh all values.
|
||
175 | * Ex. if 0x0003 is passed, bom_val[0] and bom_val[1] will be updated.
|
||
176 | *
|
||
177 | * @return 0 if init succesfull, an error code otherwise
|
||
178 | *
|
||
179 | * @see bom_get
|
||
180 | **/
|
||
181 | int bom_refresh(int bit_field) { |
||
182 | int i;
|
||
183 | int loop_was_running = 0; |
||
184 | |||
185 | if(!bom_initd)
|
||
186 | return ERROR_LIBRARY_NOT_INITD;
|
||
187 | |||
188 | |||
189 | //Check analog loop status
|
||
190 | if(analog_loop_status() == ADC_LOOP_RUNNING) {
|
||
191 | loop_was_running = 1;
|
||
192 | analog_stop_loop(); |
||
193 | } |
||
194 | |||
195 | //Read BOM values
|
||
196 | for(i = 0; i < NUM_BOM_LEDS; i++) { |
||
197 | if(bit_field & 0x1) { |
||
198 | bom_select(i); |
||
199 | bom_val[i] = analog_get8(analog_pin); |
||
200 | } |
||
201 | bit_field = bit_field >> 1;
|
||
202 | } |
||
203 | |||
204 | //Restore analog loop status
|
||
205 | if(loop_was_running)
|
||
206 | analog_start_loop(); |
||
207 | |||
208 | return 0; |
||
209 | } |
||
210 | |||
211 | /**
|
||
212 | * Gets the bom reading from bom_val[which]. Call bom_refresh beforehand to read new bom values.
|
||
213 | *
|
||
214 | * @pre must call bom refresh first
|
||
215 | *
|
||
216 | * @param which which bom value to return
|
||
217 | *
|
||
218 | * @return the bom value, -1 on error
|
||
219 | *
|
||
220 | * see bom_refresh
|
||
221 | **/
|
||
222 | int bom_get(int which) { |
||
223 | if(!bom_initd)
|
||
224 | return -1; |
||
225 | |||
226 | return bom_val[which];
|
||
227 | } |
||
228 | |||
229 | /**
|
||
230 | * Compares all the values in bom_val[] and returns the index to the lowest (max) value element.
|
||
231 | *
|
||
232 | 1443 | rcahoon | * @pre must call bom refresh
|
233 | 1461 | bneuman | * @return index to the lowest (max) bom value element. -1 if no value is lower than
|
234 | * BOM_VALUE_THRESHOLD, -2 if the bom is not initialized
|
||
235 | **/
|
||
236 | int bom_get_max(void) { |
||
237 | int i, lowest_val, lowest_i;
|
||
238 | |||
239 | if(!bom_initd)
|
||
240 | return -2; |
||
241 | |||
242 | lowest_i = -1;
|
||
243 | lowest_val = 255;
|
||
244 | for(i = 0; i < NUM_BOM_LEDS; i++) { |
||
245 | if(bom_val[i] < lowest_val) {
|
||
246 | lowest_val = bom_val[i]; |
||
247 | lowest_i = i; |
||
248 | } |
||
249 | } |
||
250 | |||
251 | if(lowest_val < BOM_VALUE_THRESHOLD)
|
||
252 | return lowest_i;
|
||
253 | else
|
||
254 | return -1; |
||
255 | } |
||
256 | |||
257 | /**
|
||
258 | * Computes the weighted average of all the bom readings to estimate the position (and distance) of another robot.
|
||
259 | *
|
||
260 | * @pre must call bom refresh
|
||
261 | * @param dist pointer to int in which to return the estimated distance to the other robot
|
||
262 | 1443 | rcahoon | * @return estimated position of the max bom value element as a fixed point value analogous to 10 times the
|
263 | 1461 | bneuman | * index of the max bom value. -1 if no value is lower than BOM_VALUE_THRESHOLD, -2 if the bom is not initialized
|
264 | 1443 | rcahoon | **/
|
265 | int bom_get_max10(int *dist) { |
||
266 | int i, max;
|
||
267 | 1445 | rcahoon | long long mean = 0, sum = 0; |
268 | 1443 | rcahoon | |
269 | 1461 | bneuman | if(!bom_initd)
|
270 | return ERROR_LIBRARY_NOT_INITD;
|
||
271 | |||
272 | |||
273 | 1443 | rcahoon | max = bom_get_max(); |
274 | if (max < 0) |
||
275 | { |
||
276 | if (dist)
|
||
277 | { |
||
278 | *dist = -1;
|
||
279 | } |
||
280 | return -1; |
||
281 | } |
||
282 | /* Record values into an array */
|
||
283 | for (i = 0; i < NUM_BOM_LEDS; i++) { |
||
284 | int idx = ((i + (NUM_BOM_LEDS/2 - max) + NUM_BOM_LEDS) % NUM_BOM_LEDS) - (NUM_BOM_LEDS/2 - max); |
||
285 | int val = 255 - bom_val[i]; |
||
286 | mean += idx * val; |
||
287 | sum += val; |
||
288 | } |
||
289 | mean = (mean * 10) / sum;
|
||
290 | mean = (mean + NUM_BOM_LEDS*10) % (NUM_BOM_LEDS*10); |
||
291 | |||
292 | if (dist)
|
||
293 | { |
||
294 | *dist = 50 - sum/48; |
||
295 | } |
||
296 | |||
297 | return mean;
|
||
298 | 1461 | bneuman | } |
299 | |||
300 | /**
|
||
301 | * Iterates through each bit in the bit_field. If the bit is set, the corresponding emitter will
|
||
302 | * be enabled to turn on when bom_on() is called.
|
||
303 | * bom_init must be called for this to work. Does nothing if a BOM1.0 is installed
|
||
304 | *
|
||
305 | * @param bit_field specifies which leds should be turned on when bom_on is called. Use BOM_ALL to turn on all bom leds.
|
||
306 | * Ex. if 0x0005 is passed, leds 0 and 2 will be turned on.
|
||
307 | *
|
||
308 | * @return 0 if init succesfull, an error code otherwise
|
||
309 | *
|
||
310 | **/
|
||
311 | int bom_set_leds(int bit_field) { |
||
312 | int i;
|
||
313 | unsigned int mask = 1<<(NUM_BOM_LEDS-1); |
||
314 | |||
315 | if(!bom_initd)
|
||
316 | return ERROR_LIBRARY_NOT_INITD;
|
||
317 | |||
318 | switch(bom_type) {
|
||
319 | case BOM10:
|
||
320 | //TODO: put an assert here to alert the user that this should not be called
|
||
321 | break;
|
||
322 | |||
323 | case BOM15:
|
||
324 | for(i=NUM_BOM_LEDS; i>0; i--) |
||
325 | { |
||
326 | //set the current bit, sending MSB first
|
||
327 | digital_output(BOM_DATA, bit_field&mask); |
||
328 | //then pulse the clock
|
||
329 | digital_output(BOM_CLOCK, 1);
|
||
330 | digital_output(BOM_CLOCK, 0);
|
||
331 | mask = mask>>1;
|
||
332 | } |
||
333 | break;
|
||
334 | |||
335 | case RBOM:
|
||
336 | //add rbom code here
|
||
337 | break;
|
||
338 | } |
||
339 | |||
340 | return 0; |
||
341 | } |
||
342 | |||
343 | |||
344 | /**
|
||
345 | * (DEPRECATED) Returns the direction of the maximum BOM reading,
|
||
346 | * as an integer in the range 0-15. 0 indicates to the
|
||
347 | * robot's right, while the rest of the sensors are
|
||
348 | * numbered counterclockwise. This is useful for determining
|
||
349 | * the direction of a robot flashing its BOM, of only one
|
||
350 | * robot is currently doing so. analog_init must be called
|
||
351 | * before this function can be used.
|
||
352 | *
|
||
353 | * @return the direction of the maximum BOM reading
|
||
354 | *
|
||
355 | * @see analog_init
|
||
356 | **/
|
||
357 | int get_max_bom(void) { |
||
358 | bom_refresh(BOM_ALL); |
||
359 | return bom_get_max();
|
||
360 | } |
||
361 | |||
362 | /**
|
||
363 | * Flashes the BOM. If using a BOM1.5, only the emitters that have been enabled using
|
||
364 | * bom_set_leds will turn on.
|
||
365 | *
|
||
366 | * @return 0 if init succesfull, an error code otherwise
|
||
367 | *
|
||
368 | * @see bom_off, bom_set_leds
|
||
369 | **/
|
||
370 | int bom_on(void) |
||
371 | { |
||
372 | if(!bom_initd)
|
||
373 | return ERROR_LIBRARY_NOT_INITD;
|
||
374 | |||
375 | switch(bom_type) {
|
||
376 | case BOM10:
|
||
377 | digital_output(MONKL, 1);
|
||
378 | break;
|
||
379 | case BOM15:
|
||
380 | digital_output(BOM_STROBE, 1);
|
||
381 | break;
|
||
382 | case RBOM:
|
||
383 | break;
|
||
384 | } |
||
385 | |||
386 | return 0; |
||
387 | } |
||
388 | |||
389 | /**
|
||
390 | * Turns off all bom leds.
|
||
391 | *
|
||
392 | * @return 0 if init succesfull, an error code otherwise
|
||
393 | *
|
||
394 | * @see bom_on
|
||
395 | **/
|
||
396 | int bom_off(void) |
||
397 | { |
||
398 | if(!bom_initd)
|
||
399 | return ERROR_LIBRARY_NOT_INITD;
|
||
400 | |||
401 | switch(bom_type) {
|
||
402 | case BOM10:
|
||
403 | digital_output(MONKL, 0);
|
||
404 | break;
|
||
405 | case BOM15:
|
||
406 | digital_output(BOM_STROBE, 0);
|
||
407 | break;
|
||
408 | case RBOM:
|
||
409 | break;
|
||
410 | } |
||
411 | |||
412 | return 0; |
||
413 | } |
||
414 | |||
415 | /** @} **/ //end group |
||
416 | |||
417 | //select a detector to read
|
||
418 | static void bom_select(char which) { |
||
419 | if(bom_type == BOM10)
|
||
420 | which = lookup[(int)which];
|
||
421 | |||
422 | if (which&8) |
||
423 | digital_output(select_pins[3], 1); |
||
424 | else
|
||
425 | digital_output(select_pins[3], 0); |
||
426 | |||
427 | if (which&4) |
||
428 | digital_output(select_pins[2], 1); |
||
429 | else
|
||
430 | digital_output(select_pins[2], 0); |
||
431 | |||
432 | if (which&2) |
||
433 | digital_output(select_pins[1], 1); |
||
434 | else
|
||
435 | digital_output(select_pins[1], 0); |
||
436 | |||
437 | if (which&1) |
||
438 | digital_output(select_pins[0], 1); |
||
439 | else
|
||
440 | digital_output(select_pins[0], 0); |
||
441 | |||
442 | } |