Statistics
| Revision:

root / demos / hunter_prey / lib / src / libdragonfly / bom.c @ 1828

History | View | Annotate | Download (17.1 KB)

1
/**
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
#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

    
47
/* *****************************
48
 * BOM Vector Component Tables *
49
 **************************** **/
50

    
51
/*
52
 * The x component of each BOM detector (indexed from 0 to 15)
53
 * was calculated using the following formula:
54
 *
55
 *                x_comp[i] = fix(25 * cos ( 2 * pi / 16 * i) )
56
 *
57
 * where "fix" rounds towards 0. If the BOM detectors were superimposed
58
 * onto a 2 dimensional Cartesian space, this effectively calculates the
59
 * x component of the emitter vector where emitter 0 corresponds to an
60
 * angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
61
 */
62
static const signed int x_comp[16] = {
63
        25,
64
        23,
65
        17,
66
        9,
67
        0,
68
        -9,
69
        -17,
70
        -23,
71
        -25,
72
        -23,
73
        -17,
74
        -9,
75
        0,
76
        9,
77
        17,
78
        23
79
};
80

    
81

    
82
/*
83
 * The y component of each BOM detector (indexed from 0 to 15)
84
 * was calculated using the following formula:
85
 *
86
 *                y_comp[i] = fix(25 * sin ( 2 * pi / 16 * i) )
87
 *
88
 * where "fix" rounds towards 0. If the BOM detectors were superimposed
89
 * onto a 2 dimensional Cartesian space, this effectively calculates the
90
 * x component of the emitter vector where emitter 0 corresponds to an
91
 * angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
92
 */
93
static signed int y_comp[16] = {
94
        0,
95
        9,
96
        17,
97
        23,
98
        25,
99
        23,
100
        17,
101
        9,
102
        0,
103
        -9,
104
        -17,
105
        -23,
106
        -25,
107
        -23,
108
        -17,
109
        -9
110
};
111

    
112
// internal function prototypes
113
static void bom_select(char which);
114

    
115
/*
116
 Bk R Y (Analog)
117
---------
118
 Green
119
 Blue
120
 White
121
---------
122
 Blue
123
 White
124
*/
125

    
126

    
127
/*
128
the analog pin definitions from dio.h DO NOT work here,
129
so we must use PF0 from avrgcc (as opposed to _PIN_F0).
130
BUT the dio pin definitions from dio.h must be used (no PE...).
131

132
also, _PIN_E2 is initialized to high for some reason,
133
which turns the BOM on when the robot is turned on.
134
WORK-AROUND: call digital_output(_PIN_E2,0) at some point.
135

136
*/
137

    
138
#define MONKI PF0         //analog (yellow)
139
//------------------------//
140
#define MONKL _PIN_E2     //green
141
#define MONK1 _PIN_E3     //blue
142
#define MONK0 _PIN_E4     //white
143
//------------------------//
144
#define MONK3 _PIN_E6     //blue
145
#define MONK2 _PIN_E7     //white
146

    
147
#define BOM_VALUE_THRESHOLD 150 //200
148
#define NUM_BOM_LEDS 16
149

    
150
/*
151
  *The following pin definitions are for the BOM v1.5
152
  */
153

    
154
#define BOM_MODE        _PIN_E2        //dio0
155
#define BOM_STROBE        _PIN_E3        //dio1
156

    
157
#define BOM_DATA        _PIN_A0 //servo0
158
#define BOM_CLOCK        _PIN_A1        //servo1
159

    
160
#define BOM_S0                _PIN_E5        //dio3
161
#define BOM_S1                _PIN_E4        //dio2
162
#define BOM_S2                _PIN_E7        //dio4
163
#define BOM_S3                _PIN_E6        //dio5
164
#define BOM_OUT                PF0                //analog(yellow)
165

    
166
/**
167
 * @defgroup bom BOM (Bearing and Orientation Module)
168
 * @brief Functions for dealing with the BOM.
169
 *
170
 * The Bearing and Orientation Module / Barrel of Monkeys / BOM
171
 * is a custom sensor designed and built by the Colony Project.
172
 * It consists of a ring of 16 IR emitters and 16 IR detectors.
173
 * The BOM is most often use to determine the direction of other
174
 * robots. This module contains functions for controlling the BOM.
175
 *
176
 * Include bom.h to access these functions.
177
 *
178
 * @{
179
 **/
180

    
181
static unsigned int bom_val[NUM_BOM_LEDS];
182
static volatile char bom_type = BOM10;
183
static int select_pins[4];
184
static int analog_pin;
185

    
186
/**
187
 * Initializes the BOM.
188
 * Call bom_init before reading bom values or turning bom leds.
189
 *
190
 * @bugs INCOMPLETE - No utilization of BOM1.5 RSSI capability. Probably leave this out
191
 * until Cornell and Pras return
192
 * 
193
 * @see bom_refresh, bom_leds_on, bom_leds_off
194
 **/
195
void bom_init(char type) {
196
    bom_type = type;
197
    
198
    switch(bom_type) {
199
    case BOM10:
200
                select_pins[0] = MONK0; 
201
                select_pins[1] = MONK1;
202
                select_pins[2] = MONK2;
203
                select_pins[3] = MONK3;
204
                analog_pin = MONKI;
205
        break;
206
    case BOM15:
207
        //Sets BOM1.5 to normal [BOM] mode
208
        digital_output(BOM_MODE, 0);
209
                select_pins[0] = BOM_S0; 
210
                select_pins[1] = BOM_S1;
211
                select_pins[2] = BOM_S2;
212
                select_pins[3] = BOM_S3;
213
                bom_set_leds(BOM_ALL);
214
                analog_pin = BOM_OUT;
215
        break;
216
    case RBOM:
217
        break;
218
    //default:
219
    }
220
}
221

    
222
/**
223
 * Iterates through each bit in the bit_field. For each set bit, sets the corresponding bom select bits
224
 *    and updates the corresponding bom value with an analog_get8 reading.  analog_init and bom_init
225
 *    must be called for this to work. Must call this before reading BOM values!
226
 *
227
 *
228
 * @param bit_field specifies which elements in bom_val[] should be updated. Use BOM_ALL to refresh all values.
229
 *    Ex. if 0x0003 is passed, bom_val[0] and bom_val[1] will be updated.
230
 *
231
 * @see bom_get
232
 **/
233
void bom_refresh(int bit_field) {
234
    int i;
235
        int loop_was_running = 0;
236
    
237
        //Check analog loop status
238
    if(analog_loop_status() == ADC_LOOP_RUNNING) {
239
                loop_was_running = 1;
240
                analog_stop_loop();
241
        }
242
    
243
        //Read BOM values
244
    for(i = 0; i < NUM_BOM_LEDS; i++) {
245
        if(bit_field & 0x1) {
246
            bom_select(i);
247
            bom_val[i] = analog_get8(analog_pin);
248
        }
249
        bit_field = bit_field >> 1;
250
    }
251
    
252
        //Restore analog loop status
253
        if(loop_was_running)
254
                analog_start_loop();
255
}
256

    
257
/**
258
 * Gets the bom reading from bom_val[which].  Call bom_refresh beforehand to read new bom values.
259
 *
260
 * @pre must call bom refresh first
261
 *
262
 * @param which which bom value to return
263
 *
264
 * @return the bom value
265
 *
266
 * see bom_refresh
267
 **/
268
int bom_get(int which) {
269
    return bom_val[which];
270
}
271

    
272
/** 
273
 * Compares all the values in bom_val[] and returns the index to the lowest (max) value element.
274
 *
275
 * @pre must call bom refresh
276
 * @return index to the lowest (max) bom value element.  -1 if no value is lower than
277
 *    BOM_VALUE_THRESHOLD
278
 **/
279
int bom_get_max(void) {
280
    int i, lowest_val, lowest_i;
281
    lowest_i = -1;
282
    lowest_val = 255;
283
    for(i = 0; i < NUM_BOM_LEDS; i++) {
284
        if(bom_val[i] < lowest_val) {
285
            lowest_val = bom_val[i];
286
            lowest_i = i;
287
        }
288
    }
289
    
290
    if(lowest_val < BOM_VALUE_THRESHOLD)
291
        return lowest_i;
292
    else
293
        return -1;
294
}
295

    
296
/** 
297
 * Compute the net resultant BOM IR vector by scaling each IR unit vector by its intensity
298
 *                and summing over all IR LEDs.
299
 *
300
 * @param v  Pointer to Vector struct to be filled by this function with an x and y net vector
301
 *                component.
302
 *
303
 * @param usrBOMvals  Pointer to array which holds 16 raw BOM readings. Can be used if user
304
 *                has already collected BOM information. Otherwise, leave as NULL and a new set of data
305
 *                will be collected and used.
306
 *
307
 * @return  Exit status - Zero for success; negative on error.
308
 **/
309
int bom_get_vector(Vector* v, int* usrBOMvals) {
310

    
311
        /* Store current BOM readings and use them as a weighting factor */
312
        int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
313

    
314
        /* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")
315
         * components. Calculated by multiplying the intensity by the x and y
316
         * component respectively (x and y components are stored in the tables
317
         * above). */
318
        int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
319
        int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
320
        
321
        /* Accumulators to sum up the net x ("Rightness") and y ("Forwardness")
322
         * components for the entire robot. */
323
        long net_x_comp = 0;
324
        long net_y_comp = 0;
325

    
326
        int i = 0;
327

    
328
        /* BOM intensity is actually measured as more intense = closer to 0 */
329
        if (usrBOMvals) {
330
                /* Use BOM values collected by user */
331
                for (i = 0; i < 16; i++) {
332
                        intensity[i] = 255 - usrBOMvals[i];
333
                }
334
        } else {
335
                /* Collect new set of BOM data */
336
                bom_refresh(BOM_ALL);
337
                for (i = 0; i < 16; i++) {
338
                        intensity[i] = 255 - bom_get(i);
339
                }
340
        }
341

    
342
        /* Calculate weighted vector components and accumulate vector sum */
343
        for (i = 0; i < 16; i++) {
344
                weighted_x_comp[i] = intensity[i] * x_comp[i];
345
                weighted_y_comp[i] = intensity[i] * y_comp[i];
346
                net_x_comp += weighted_x_comp[i];
347
                net_y_comp += weighted_y_comp[i];
348
        }
349

    
350
        /* Fill the Vector struct */
351
        v->x = net_x_comp;
352
        v->y = net_y_comp;
353

    
354
        return 0;
355

    
356
}
357

    
358
/** 
359
 * Compute the normalized net resultant BOM IR vector by scaling each IR unit vector by its
360
 *                intensity and summing over all IR LEDs.
361
 *
362
 * @param v  Pointer to Vector struct to be filled by this function with an x and y net vector
363
 *                component.
364
 *
365
 * @param usrBOMvals  Pointer to array which holds 16 raw BOM readings. Can be used if user
366
 *                has already collected BOM information. Otherwise, leave as NULL and a new set of data
367
 *                will be collected and used.
368
 *
369
 * @return  Exit status - Zero for success; negative on error.
370
 **/
371
int bom_get_norm_vector(Vector* v, int* usrBOMvals) {
372

    
373
        /* Store current BOM readings and use them as a weighting factor */
374
        int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
375

    
376
        /* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")
377
         * components. Calculated by multiplying the intensity by the x and y
378
         * component respectively (x and y components are stored in the tables
379
         * above). */
380
        int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
381
        int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
382
        
383
        /* Accumulators to sum up the net x ("Rightness") and y ("Forwardness")
384
         * components for the entire robot. */
385
        long net_x_comp = 0;
386
        long net_y_comp = 0;
387

    
388
        /* Variables used to normalize the net component values */
389
        int total_intensity = 0;
390
        int normalized_net_x_comp = 0;
391
        int normalized_net_y_comp = 0;
392

    
393
        int i = 0;
394

    
395
        /* BOM intensity is actually measured as more intense = closer to 0 */
396
        if (usrBOMvals) {
397
                /* Use BOM values collected by user */
398
                for (i = 0; i < 16; i++) {
399
                        intensity[i] = 255 - usrBOMvals[i];
400
                }
401
        } else {
402
                /* Collect new set of BOM data */
403
                bom_refresh(BOM_ALL);
404
                for (i = 0; i < 16; i++) {
405
                        intensity[i] = 255 - bom_get(i);
406
                }
407
        }
408

    
409
        /* Calculate weighted vector components and accumulate vector sum */
410
        for (i = 0; i < 16; i++) {
411
                weighted_x_comp[i] = intensity[i] * x_comp[i];
412
                weighted_y_comp[i] = intensity[i] * y_comp[i];
413
                net_x_comp += weighted_x_comp[i];
414
                net_y_comp += weighted_y_comp[i];
415
                total_intensity += intensity[i];
416
        }
417

    
418
        /* Normalize the resultant vector components by the total intensity */
419
        if (total_intensity > 0) {
420
                normalized_net_x_comp = net_x_comp / total_intensity;
421
                normalized_net_y_comp = net_y_comp / total_intensity;
422
        }
423

    
424
        /* Fill the Vector struct */
425
        v->x = normalized_net_x_comp;
426
        v->y = normalized_net_y_comp;
427

    
428
        return 0;
429

    
430
}
431

    
432
/** 
433
 * Print a histogram which shows the current BOM intensity values for each of the 16 BOM IR
434
 *                sensors. The function will attempt to send the histogram data over USB.
435
 *
436
 * @param curBOMvals  Pointer to an array of the current BOM values (the array must have
437
 *                length 16). Use this to print values you have already collected. Otherwise pass in NULL
438
 *                and bom_refresh() will be called and the current BOM intensity values will be collected.
439
 * @return  Exit status - Zero for success; negative on error.
440
 **/
441
int bom_print_usb(int* usrBOMvals) {
442

    
443
        int i, j, max = -1;
444
        int curVals[16];
445
        int* prtValPtr;
446

    
447
        if (usrBOMvals) {
448
                /* Use BOM values collected by user */
449
                prtValPtr = usrBOMvals;
450

    
451
                /* Find max BOM value from users values */
452
                for (i = 0; i < 16; i++) {
453
                        if (max < prtValPtr[i])
454
                                max = prtValPtr[i];
455
                }
456
        } else {
457
                /* Refresh and make sure the table is updated */
458
                bom_refresh(BOM_ALL);
459

    
460
                /* Record values into an array */
461
                for (i = 0; i < 16; i++) {
462
                        curVals[i] = bom_get(i);
463
                        if (max < curVals[i])
464
                                max = curVals[i];
465
                }
466

    
467
                /* Use the current set of collected values */
468
                prtValPtr = curVals;
469
        }
470

    
471
        /* Display results */
472
        for (i = 0; i < 16; i++) {
473
                
474
                usb_puti(i);
475
                usb_puts(": ");
476
                usb_puti(prtValPtr[i]);
477
                usb_putc('\t');
478

    
479
                for (j = 0; j < (int)((max - prtValPtr[i]) / 5); j++) {
480
                        usb_putc('#');
481
                }
482
                usb_puts("\r\n");
483
        }
484
        usb_puts("\r\n");
485

    
486
        return 0;
487

    
488
}
489

    
490
/** 
491
 * Computes the weighted average of all the bom readings to estimate the position (and distance) of another robot.
492
 *
493
 * @pre must call bom refresh
494
 * @param dist  pointer to int in which to return the estimated distance to the other robot
495
 * @return estimated position of the max bom value element as a fixed point value analogous to 10 times the
496
 *        index of the max bom value.  -1 if no value is lower than BOM_VALUE_THRESHOLD.
497
 **/
498
int bom_get_max10(int *dist) {
499
    int i, max;
500
    long long mean = 0, sum = 0;
501

    
502
    max = bom_get_max();
503
    if (max < 0)
504
    {
505
        if (dist)
506
        {
507
            *dist = -1;
508
        }
509
        return -1;
510
    }
511
    /* Record values into an array */
512
    for (i = 0; i < NUM_BOM_LEDS; i++) {
513
        int idx = ((i + (NUM_BOM_LEDS/2 - max) + NUM_BOM_LEDS) % NUM_BOM_LEDS) - (NUM_BOM_LEDS/2 - max);
514
        int val = 255 - bom_val[i];
515
        mean += idx * val;
516
        sum += val;
517
    }
518
    mean = (mean * 10) / sum;
519
    mean = (mean + NUM_BOM_LEDS*10) % (NUM_BOM_LEDS*10);
520

    
521
    if (dist)
522
    {
523
        *dist = 50 - sum/48;
524
    }
525

    
526
    return mean;
527
}
528

    
529
/**
530
 * Iterates through each bit in the bit_field. If the bit is set, the corresponding emitter will
531
 *    be enabled to turn on when bom_on() is called.
532
 *    bom_init must be called for this to work. Does nothing if a BOM1.0 is installed
533
 *
534
 * @param bit_field specifies which leds should be turned on when bom_on is called.  Use BOM_ALL to turn on all bom leds.
535
 *    Ex. if 0x0005 is passed, leds 0 and 2 will be turned on.
536
 **/
537
void bom_set_leds(int bit_field) {
538
    int i;
539
        unsigned int mask = 1<<(NUM_BOM_LEDS-1);
540
        switch(bom_type) {
541
    case BOM10:
542
        //TODO: put an assert here to alert the user that this should not be called
543
        break;
544
                
545
    case BOM15:
546
            for(i=NUM_BOM_LEDS; i>0; i--)
547
            {
548
                    //set the current bit, sending MSB first
549
                    digital_output(BOM_DATA, bit_field&mask);
550
                    //then pulse the clock
551
                    digital_output(BOM_CLOCK, 1);
552
                    digital_output(BOM_CLOCK, 0);
553
                        mask = mask>>1;
554
            }
555
        break;
556
                
557
    case RBOM:
558
        //add rbom code here
559
        break;
560
    }
561
}
562

    
563

    
564
/**
565
 * (DEPRECATED) Returns the direction of the maximum BOM reading,
566
 * as an integer in the range 0-15. 0 indicates to the
567
 * robot's right, while the rest of the sensors are
568
 * numbered counterclockwise. This is useful for determining
569
 * the direction of a robot flashing its BOM, of only one
570
 * robot is currently doing so. analog_init must be called
571
 * before this function can be used.
572
 *
573
 * @return the direction of the maximum BOM reading
574
 *
575
 * @see analog_init
576
 **/
577
int get_max_bom(void) {
578
    bom_refresh(BOM_ALL);
579
    return bom_get_max();
580
}
581

    
582
/**
583
 * Flashes the BOM.  If using a BOM1.5, only the emitters that have been enabled using
584
 * bom_set_leds will turn on.
585
 * 
586
 * @see bom_off, bom_set_leds
587
 **/
588
void bom_on(void)
589
{
590
  switch(bom_type) {
591
  case BOM10:
592
        digital_output(MONKL, 1);
593
        break;
594
  case BOM15:
595
        digital_output(BOM_STROBE, 1);
596
        break;
597
  case RBOM:
598
        break;
599
  }
600
}
601

    
602
/**
603
 * Turns off all bom leds.
604
 * 
605
 * @see bom_on
606
 **/
607
void bom_off(void)
608
{
609
  switch(bom_type) {
610
  case BOM10:
611
        digital_output(MONKL, 0);
612
        break;
613
  case BOM15:
614
        digital_output(BOM_STROBE, 0);
615
        break;
616
  case RBOM:
617
        break;
618
  }
619
}
620

    
621
/** @} **/ //end group
622

    
623
//select a detector to read
624
static void bom_select(char which) {
625
        if(bom_type == BOM10)
626
          which = lookup[(int)which];
627
        
628
    if (which&8)
629
      digital_output(select_pins[3], 1);
630
    else
631
      digital_output(select_pins[3], 0);
632

    
633
    if (which&4)
634
      digital_output(select_pins[2], 1);
635
    else
636
      digital_output(select_pins[2], 0);
637

    
638
    if (which&2)
639
      digital_output(select_pins[1], 1);
640
    else
641
      digital_output(select_pins[1], 0);
642

    
643
    if (which&1)
644
      digital_output(select_pins[0], 1);
645
    else
646
      digital_output(select_pins[0], 0);
647
        
648
}