root / branches / simulator / projects / libdragonfly / bom.c @ 943
History | View | Annotate | Download (4.37 KB)
1 | 891 | 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 | 448 | cmar | **/
|
25 | |||
26 | 891 | bcoltin | |
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 | 943 | bcoltin | #include <avr/io.h> |
42 | |||
43 | 891 | bcoltin | //constants
|
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 200 |
||
82 | #define NUM_BOM_LEDS 16 |
||
83 | |||
84 | |||
85 | |||
86 | 448 | cmar | static unsigned int bom_val[NUM_BOM_LEDS]; |
87 | static char bom_type = BOM; |
||
88 | |||
89 | void bom_init(char type) { |
||
90 | bom_type = type; |
||
91 | |||
92 | switch(bom_type) {
|
||
93 | case BOM:
|
||
94 | break;
|
||
95 | case BOM15:
|
||
96 | break;
|
||
97 | case RBOM:
|
||
98 | break;
|
||
99 | //default:
|
||
100 | } |
||
101 | } |
||
102 | |||
103 | void bom_refresh(int bit_field) { |
||
104 | int i;
|
||
105 | |||
106 | analog_stop_loop(); |
||
107 | |||
108 | for(i = 0; i < NUM_BOM_LEDS; i++) { |
||
109 | if(bit_field & 0x1) { |
||
110 | bom_select(lookup[i]); |
||
111 | bom_val[i] = analog_get8(MONKI); |
||
112 | } |
||
113 | bit_field = bit_field >> 1;
|
||
114 | } |
||
115 | |||
116 | analog_start_loop(); |
||
117 | } |
||
118 | |||
119 | int bom_get(int which) { |
||
120 | return bom_val[which];
|
||
121 | } |
||
122 | |||
123 | int bom_get_max(void) { |
||
124 | int i, lowest_val, lowest_i;
|
||
125 | lowest_i = -1;
|
||
126 | 891 | bcoltin | lowest_val = 255;
|
127 | for(i = 0; i < NUM_BOM_LEDS; i++) { |
||
128 | 448 | cmar | if(bom_val[i] < lowest_val) {
|
129 | lowest_val = bom_val[i]; |
||
130 | lowest_i = i; |
||
131 | } |
||
132 | 491 | cmar | } |
133 | 448 | cmar | |
134 | if(lowest_val < BOM_VALUE_THRESHOLD)
|
||
135 | return lowest_i;
|
||
136 | else
|
||
137 | return -1; |
||
138 | } |
||
139 | |||
140 | void bom_leds_on(int bit_field) { |
||
141 | switch(bom_type) {
|
||
142 | case BOM:
|
||
143 | if(bit_field == BOM_ALL) {
|
||
144 | digital_output(MONKL, 1);
|
||
145 | } |
||
146 | break;
|
||
147 | case BOM15:
|
||
148 | //add bom 1.5 code here
|
||
149 | break;
|
||
150 | case RBOM:
|
||
151 | //add rbom code here
|
||
152 | break;
|
||
153 | 891 | bcoltin | } |
154 | 448 | cmar | } |
155 | |||
156 | void bom_leds_off(int bit_field) { |
||
157 | switch(bom_type) {
|
||
158 | case BOM:
|
||
159 | if(bit_field == BOM_ALL) {
|
||
160 | digital_output(MONKL, 0);
|
||
161 | } |
||
162 | break;
|
||
163 | case BOM15:
|
||
164 | //add bom 1.5 code here
|
||
165 | break;
|
||
166 | case RBOM:
|
||
167 | //add rbom code here
|
||
168 | break;
|
||
169 | 891 | bcoltin | } |
170 | 448 | cmar | } |
171 | 891 | bcoltin | |
172 | void bom_on(void) |
||
173 | { |
||
174 | bom_leds_on(BOM_ALL); |
||
175 | } |
||
176 | |||
177 | void bom_off(void) |
||
178 | { |
||
179 | bom_leds_off(BOM_ALL); |
||
180 | } |
||
181 | |||
182 | 448 | cmar | static void bom_select(char which) { |
183 | 891 | bcoltin | if (which&8) |
184 | digital_output(MONK3, 1);
|
||
185 | else
|
||
186 | digital_output(MONK3, 0);
|
||
187 | |||
188 | if (which&4) |
||
189 | digital_output(MONK2, 1);
|
||
190 | else
|
||
191 | digital_output(MONK2, 0);
|
||
192 | |||
193 | if (which&2) |
||
194 | digital_output(MONK1, 1);
|
||
195 | else
|
||
196 | digital_output(MONK1, 0);
|
||
197 | |||
198 | if (which&1) |
||
199 | digital_output(MONK0, 1);
|
||
200 | else
|
||
201 | 448 | cmar | digital_output(MONK0, 0);
|
202 | } |