Statistics
| Revision:

root / demos / john / projects / libdragonfly / bom.c @ 1692

History | View | Annotate | Download (17.2 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];      deleted because we got rid of the analog loop
270
        bom_select(which);
271
    return analog_get8(analog_pin);
272
}
273

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

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

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

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

    
329
        int i = 0;
330

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

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

    
353
        /* Fill the Vector struct */
354
        v->x = net_x_comp;
355
        v->y = net_y_comp;
356

    
357
        return 0;
358

    
359
}
360

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

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

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

    
391
        /* Variables used to normalize the net component values */
392
        int total_intensity = 0;
393
        int normalized_net_x_comp = 0;
394
        int normalized_net_y_comp = 0;
395

    
396
        int i = 0;
397

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

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

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

    
427
        /* Fill the Vector struct */
428
        v->x = normalized_net_x_comp;
429
        v->y = normalized_net_y_comp;
430

    
431
        return 0;
432

    
433
}
434

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

    
446
        int i, j, max = -1;
447
        int curVals[16];
448
        int* prtValPtr;
449

    
450
        if (usrBOMvals) {
451
                /* Use BOM values collected by user */
452
                prtValPtr = usrBOMvals;
453

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

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

    
470
                /* Use the current set of collected values */
471
                prtValPtr = curVals;
472
        }
473

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

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

    
489
        return 0;
490

    
491
}
492

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

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

    
524
    if (dist)
525
    {
526
        *dist = 50 - sum/48;
527
    }
528

    
529
    return mean;
530
}
531

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

    
566

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

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

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

    
624
/** @} **/ //end group
625

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

    
636
    if (which&4)
637
      digital_output(select_pins[2], 1);
638
    else
639
      digital_output(select_pins[2], 0);
640

    
641
    if (which&2)
642
      digital_output(select_pins[1], 1);
643
    else
644
      digital_output(select_pins[1], 0);
645

    
646
    if (which&1)
647
      digital_output(select_pins[0], 1);
648
    else
649
      digital_output(select_pins[0], 0);
650
        
651
}