Revision 1461
updated all the library code to have sensible _init behavior.
Almost all of the library components have a global variable which gets set after init and the functions inside will fail with an error code if init has not been called. Also, the init functions themselves check this variable and will bail out without doing any damage if that init has already been called
bom.c | ||
---|---|---|
3 | 3 |
* |
4 | 4 |
* Permission is hereby granted, free of charge, to any person |
5 | 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 "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 |
static unsigned int bom_val[NUM_BOM_LEDS]; |
|
116 |
static volatile char bom_type = BOM10; |
|
117 |
static int select_pins[4]; |
|
118 |
static int analog_pin; |
|
119 |
|
|
120 |
/** |
|
121 |
* Initializes the BOM. |
|
122 |
* Call bom_init before reading bom values or turning bom leds. |
|
123 |
* |
|
124 |
* @bugs INCOMPLETE - No utilization of BOM1.5 RSSI capability. Probably leave this out |
|
125 |
* until Cornell and Pras return |
|
126 |
* |
|
127 |
* @see bom_refresh, bom_leds_on, bom_leds_off |
|
128 |
**/ |
|
129 |
void bom_init(char type) { |
|
130 |
bom_type = type; |
|
131 |
|
|
132 |
switch(bom_type) { |
|
133 |
case BOM10: |
|
134 |
select_pins[0] = MONK0; |
|
135 |
select_pins[1] = MONK1; |
|
136 |
select_pins[2] = MONK2; |
|
137 |
select_pins[3] = MONK3; |
|
138 |
analog_pin = MONKI; |
|
139 |
break; |
|
140 |
case BOM15: |
|
141 |
//Sets BOM1.5 to normal [BOM] mode |
|
142 |
digital_output(BOM_MODE, 0); |
|
143 |
select_pins[0] = BOM_S0; |
|
144 |
select_pins[1] = BOM_S1; |
|
145 |
select_pins[2] = BOM_S2; |
|
146 |
select_pins[3] = BOM_S3; |
|
147 |
bom_set_leds(BOM_ALL); |
|
148 |
analog_pin = BOM_OUT; |
|
149 |
break; |
|
150 |
case RBOM: |
|
151 |
break; |
|
152 |
//default: |
|
153 |
} |
|
154 |
} |
|
155 |
|
|
156 |
/** |
|
157 |
* Iterates through each bit in the bit_field. For each set bit, sets the corresponding bom select bits |
|
158 |
* and updates the corresponding bom value with an analog_get8 reading. analog_init and bom_init |
|
159 |
* must be called for this to work. Must call this before reading BOM values! |
|
160 |
* |
|
161 |
* |
|
162 |
* @param bit_field specifies which elements in bom_val[] should be updated. Use BOM_ALL to refresh all values. |
|
163 |
* Ex. if 0x0003 is passed, bom_val[0] and bom_val[1] will be updated. |
|
164 |
* |
|
165 |
* @see bom_get |
|
166 |
**/ |
|
167 |
void bom_refresh(int bit_field) { |
|
168 |
int i; |
|
169 |
int loop_was_running = 0; |
|
170 |
|
|
171 |
//Check analog loop status |
|
172 |
if(analog_loop_status() == ADC_LOOP_RUNNING) { |
|
173 |
loop_was_running = 1; |
|
174 |
analog_stop_loop(); |
|
175 |
} |
|
176 |
|
|
177 |
//Read BOM values |
|
178 |
for(i = 0; i < NUM_BOM_LEDS; i++) { |
|
179 |
if(bit_field & 0x1) { |
|
180 |
bom_select(i); |
|
181 |
bom_val[i] = analog_get8(analog_pin); |
|
182 |
} |
|
183 |
bit_field = bit_field >> 1; |
|
184 |
} |
|
185 |
|
|
186 |
//Restore analog loop status |
|
187 |
if(loop_was_running) |
|
188 |
analog_start_loop(); |
|
189 |
} |
|
190 |
|
|
191 |
/** |
|
192 |
* Gets the bom reading from bom_val[which]. Call bom_refresh beforehand to read new bom values. |
|
193 |
* |
|
194 |
* @pre must call bom refresh first |
|
195 |
* |
|
196 |
* @param which which bom value to return |
|
197 |
* |
|
198 |
* @return the bom value |
|
199 |
* |
|
200 |
* see bom_refresh |
|
201 |
**/ |
|
202 |
int bom_get(int which) { |
|
203 |
return bom_val[which]; |
|
204 |
} |
|
205 |
|
|
206 |
/** |
|
207 |
* Compares all the values in bom_val[] and returns the index to the lowest (max) value element. |
|
208 |
* |
|
209 |
* @pre must call bom refresh |
|
210 |
* @return index to the lowest (max) bom value element. -1 if no value is lower than |
|
211 |
* BOM_VALUE_THRESHOLD |
|
212 |
**/ |
|
213 |
int bom_get_max(void) { |
|
214 |
int i, lowest_val, lowest_i; |
|
215 |
lowest_i = -1; |
|
216 |
lowest_val = 255; |
|
217 |
for(i = 0; i < NUM_BOM_LEDS; i++) { |
|
218 |
if(bom_val[i] < lowest_val) { |
|
219 |
lowest_val = bom_val[i]; |
|
220 |
lowest_i = i; |
|
221 |
} |
|
222 |
} |
|
223 |
|
|
224 |
if(lowest_val < BOM_VALUE_THRESHOLD) |
|
225 |
return lowest_i; |
|
226 |
else |
|
227 |
return -1; |
|
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 "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; |
|
228 | 166 |
} |
229 | 167 |
|
230 |
/** |
|
231 |
* Computes the weighted average of all the bom readings to estimate the position (and distance) of another robot. |
|
232 |
* |
|
168 |
/** |
|
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 |
* |
|
233 | 232 |
* @pre must call bom refresh |
234 |
* @param dist pointer to int in which to return the estimated distance to the other robot |
|
233 |
* @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 |
|
235 | 262 |
* @return estimated position of the max bom value element as a fixed point value analogous to 10 times the |
236 |
* index of the max bom value. -1 if no value is lower than BOM_VALUE_THRESHOLD.
|
|
263 |
* index of the max bom value. -1 if no value is lower than BOM_VALUE_THRESHOLD, -2 if the bom is not initialized
|
|
237 | 264 |
**/ |
238 | 265 |
int bom_get_max10(int *dist) { |
239 | 266 |
int i, max; |
240 | 267 |
long long mean = 0, sum = 0; |
241 | 268 |
|
269 |
if(!bom_initd) |
|
270 |
return ERROR_LIBRARY_NOT_INITD; |
|
271 |
|
|
272 |
|
|
242 | 273 |
max = bom_get_max(); |
243 | 274 |
if (max < 0) |
244 | 275 |
{ |
... | ... | |
264 | 295 |
} |
265 | 296 |
|
266 | 297 |
return mean; |
267 |
} |
|
268 |
|
|
269 |
/** |
|
270 |
* Iterates through each bit in the bit_field. If the bit is set, the corresponding emitter will |
|
271 |
* be enabled to turn on when bom_on() is called. |
|
272 |
* bom_init must be called for this to work. Does nothing if a BOM1.0 is installed |
|
273 |
* |
|
274 |
* @param bit_field specifies which leds should be turned on when bom_on is called. Use BOM_ALL to turn on all bom leds. |
|
275 |
* Ex. if 0x0005 is passed, leds 0 and 2 will be turned on. |
|
276 |
**/ |
|
277 |
void bom_set_leds(int bit_field) { |
|
278 |
int i; |
|
279 |
unsigned int mask = 1<<(NUM_BOM_LEDS-1); |
|
280 |
switch(bom_type) { |
|
281 |
case BOM10: |
|
282 |
//TODO: put an assert here to alert the user that this should not be called |
|
283 |
break; |
|
284 |
|
|
285 |
case BOM15: |
|
286 |
for(i=NUM_BOM_LEDS; i>0; i--) |
|
287 |
{ |
|
288 |
//set the current bit, sending MSB first |
|
289 |
digital_output(BOM_DATA, bit_field&mask); |
|
290 |
//then pulse the clock |
|
291 |
digital_output(BOM_CLOCK, 1); |
|
292 |
digital_output(BOM_CLOCK, 0); |
|
293 |
mask = mask>>1; |
|
294 |
} |
|
295 |
break; |
|
296 |
|
|
297 |
case RBOM: |
|
298 |
//add rbom code here |
|
299 |
break; |
|
300 |
} |
|
301 |
} |
|
302 |
|
|
303 |
|
|
304 |
/** |
|
305 |
* (DEPRECATED) Returns the direction of the maximum BOM reading, |
|
306 |
* as an integer in the range 0-15. 0 indicates to the |
|
307 |
* robot's right, while the rest of the sensors are |
|
308 |
* numbered counterclockwise. This is useful for determining |
|
309 |
* the direction of a robot flashing its BOM, of only one |
|
310 |
* robot is currently doing so. analog_init must be called |
|
311 |
* before this function can be used. |
|
312 |
* |
|
313 |
* @return the direction of the maximum BOM reading |
|
314 |
* |
|
315 |
* @see analog_init |
|
316 |
**/ |
|
317 |
int get_max_bom(void) { |
|
318 |
bom_refresh(BOM_ALL); |
|
319 |
return bom_get_max(); |
|
320 |
} |
|
321 |
|
|
322 |
/** |
|
323 |
* Flashes the BOM. If using a BOM1.5, only the emitters that have been enabled using |
|
324 |
* bom_set_leds will turn on. |
|
325 |
* |
|
326 |
* @see bom_off, bom_set_leds |
|
327 |
**/ |
|
328 |
void bom_on(void) |
|
329 |
{ |
|
330 |
switch(bom_type) { |
|
331 |
case BOM10: |
|
332 |
digital_output(MONKL, 1); |
|
333 |
break; |
|
334 |
case BOM15: |
|
335 |
digital_output(BOM_STROBE, 1); |
|
336 |
break; |
|
337 |
case RBOM: |
|
338 |
break; |
|
339 |
} |
|
340 |
} |
|
341 |
|
|
342 |
/** |
|
343 |
* Turns off all bom leds. |
|
344 |
* |
|
345 |
* @see bom_on |
|
346 |
**/ |
|
347 |
void bom_off(void) |
|
348 |
{ |
|
349 |
switch(bom_type) { |
|
350 |
case BOM10: |
|
351 |
digital_output(MONKL, 0); |
|
352 |
break; |
|
353 |
case BOM15: |
|
354 |
digital_output(BOM_STROBE, 0); |
|
355 |
break; |
|
356 |
case RBOM: |
|
357 |
break; |
|
358 |
} |
|
359 |
} |
|
360 |
|
|
361 |
/** @} **/ //end group |
|
362 |
|
|
363 |
//select a detector to read |
|
364 |
static void bom_select(char which) { |
|
365 |
if(bom_type == BOM10) |
|
366 |
which = lookup[(int)which]; |
|
367 |
|
|
368 |
if (which&8) |
|
369 |
digital_output(select_pins[3], 1); |
|
370 |
else |
|
371 |
digital_output(select_pins[3], 0); |
|
372 |
|
|
373 |
if (which&4) |
|
374 |
digital_output(select_pins[2], 1); |
|
375 |
else |
|
376 |
digital_output(select_pins[2], 0); |
|
377 |
|
|
378 |
if (which&2) |
|
379 |
digital_output(select_pins[1], 1); |
|
380 |
else |
|
381 |
digital_output(select_pins[1], 0); |
|
382 |
|
|
383 |
if (which&1) |
|
384 |
digital_output(select_pins[0], 1); |
|
385 |
else |
|
386 |
digital_output(select_pins[0], 0); |
|
387 |
|
|
388 |
} |
|
298 |
} |
|
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 |
} |
Also available in: Unified diff