root / trunk / code / lib / src / libdragonfly / bom.c @ 7
History | View | Annotate | Download (3.35 KB)
1 | 7 | bcoltin | #include <dragonfly_lib.h> |
---|---|---|---|
2 | #include "bom.h" |
||
3 | #include "dio.h" |
||
4 | #include "analog.h" |
||
5 | |||
6 | //constants
|
||
7 | const int lookup[16] = {7,6,5,0xe,1,4,3,2,0xf,0,0xd,8,0xc,0xb,9,0xa}; |
||
8 | |||
9 | // internal function prototypes
|
||
10 | void output_high(int which); |
||
11 | void output_low(int which); |
||
12 | |||
13 | /*
|
||
14 | Bk R Y (Analog)
|
||
15 | ---------
|
||
16 | Green
|
||
17 | Blue
|
||
18 | White
|
||
19 | ---------
|
||
20 | Blue
|
||
21 | White
|
||
22 | */
|
||
23 | |||
24 | |||
25 | /*
|
||
26 | the analog pin definitions from dio.h DO NOT work here,
|
||
27 | so we must use PF0 from avrgcc (as opposed to _PIN_F0).
|
||
28 | BUT the dio pin definitions from dio.h must be used (no PE...).
|
||
29 | |||
30 | also, _PIN_E2 is initialized to high for some reason,
|
||
31 | which turns the BOM on when the robot is turned on.
|
||
32 | WORK-AROUND: call digital_output(_PIN_E2,0) at some point.
|
||
33 | |||
34 | */
|
||
35 | |||
36 | #define MONKI PF0 //analog (yellow) |
||
37 | //------------------------//
|
||
38 | #define MONKL _PIN_E2 //green |
||
39 | #define MONK1 _PIN_E3 //blue |
||
40 | #define MONK0 _PIN_E4 //white |
||
41 | //------------------------//
|
||
42 | #define MONK3 _PIN_E6 //blue |
||
43 | #define MONK2 _PIN_E7 //white |
||
44 | |||
45 | #define BOM_VALUE_THRESHOLD 200 |
||
46 | |||
47 | |||
48 | /**
|
||
49 | * @defgroup bom BOM (Bearing and Orientation Module)
|
||
50 | * @brief Functions for dealing with the BOM.
|
||
51 | *
|
||
52 | * The Bearing and Orientation Module / Barrel of Monkeys / BOM
|
||
53 | * is a custom sensor designed and built by the Colony Project.
|
||
54 | * It consists of a ring of 16 IR emitters and 16 IR detectors.
|
||
55 | * The BOM is most often use to determine the direction of other
|
||
56 | * robots. This module contains functions for controlling the BOM.
|
||
57 | *
|
||
58 | * Include bom.h to access these functions.
|
||
59 | *
|
||
60 | * @{
|
||
61 | **/
|
||
62 | |||
63 | /**
|
||
64 | * Returns the direction of the maximum BOM reading,
|
||
65 | * as an integer in the range 0-15. 0 indicates to the
|
||
66 | * robot's right, while the rest of the sensors are
|
||
67 | * numbered counterclockwise. This is useful for determining
|
||
68 | * the direction of a robot flashing its BOM, of only one
|
||
69 | * robot is currently doing so. analog_init must be called
|
||
70 | * before this function can be used.
|
||
71 | *
|
||
72 | * @return the direction of the maximum BOM reading
|
||
73 | *
|
||
74 | * @see analog_init
|
||
75 | **/
|
||
76 | int get_max_bom(void) { |
||
77 | int max_bom_temp = 0; |
||
78 | int a, i, j, h;
|
||
79 | h = 255;
|
||
80 | |||
81 | for (j = 0; j < 16; j++) |
||
82 | { |
||
83 | i = lookup[j]; |
||
84 | |||
85 | if (i&8) |
||
86 | output_high(MONK3); |
||
87 | else
|
||
88 | output_low(MONK3); |
||
89 | |||
90 | if (i&4) |
||
91 | output_high(MONK2); |
||
92 | else
|
||
93 | output_low(MONK2); |
||
94 | |||
95 | if (i&2) |
||
96 | output_high(MONK1); |
||
97 | else
|
||
98 | output_low(MONK1); |
||
99 | |||
100 | if (i&1) |
||
101 | output_high(MONK0); |
||
102 | else
|
||
103 | output_low(MONK0); |
||
104 | |||
105 | a = analog8(MONKI); |
||
106 | |||
107 | if (a < h)
|
||
108 | { |
||
109 | h = a; |
||
110 | max_bom_temp = j; |
||
111 | } |
||
112 | |||
113 | } |
||
114 | |||
115 | //threshold on the bom analog value.
|
||
116 | //defined in bom.h
|
||
117 | // if the analog value read is above the threshold, we cannot see a robot
|
||
118 | // (remember, low means higher intensity).
|
||
119 | if(h < BOM_VALUE_THRESHOLD) {
|
||
120 | return max_bom_temp;
|
||
121 | } |
||
122 | else
|
||
123 | return -1; |
||
124 | } |
||
125 | |||
126 | /**
|
||
127 | * Flashes the BOM. analog_init must be called before this
|
||
128 | * function can be used.
|
||
129 | *
|
||
130 | * @see bom_off, analog_init
|
||
131 | **/
|
||
132 | void bom_on(void) |
||
133 | { |
||
134 | output_high(MONKL); |
||
135 | } |
||
136 | |||
137 | /**
|
||
138 | * Stops flashing the BOM. analog_init must be called
|
||
139 | * before this function can be used.
|
||
140 | *
|
||
141 | * @see bom_on, analog_init
|
||
142 | **/
|
||
143 | void bom_off(void) |
||
144 | { |
||
145 | output_low(MONKL); |
||
146 | } |
||
147 | |||
148 | /** @} **/ //end group |
||
149 | |||
150 | void output_high(int which) { |
||
151 | digital_output(which, 1);
|
||
152 | } |
||
153 | |||
154 | void output_low(int which) { |
||
155 | digital_output(which, 0);
|
||
156 | } |